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读者 服务 

轻松 注册 成 为 博文 视点 社区 用 刻 (www.broadview.com.cn) ， 您 即 
可 至 受 以 下 服务 。 

«FAQ: 本 书 所 提供 的 示例 代码 及 资源 文件 均 可 在 下 载 资 源 处 
下 载 。 

JESHI: ”您 对 书 中 内 容 的 修改 意见 可 在 提交 勘误 处 提交 ， 帮 被 
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第 1 讲 ”预备 知识 


1.1 本 书 讲 什么 


这 是 一 本 介绍 视觉 SLAM 的 书 ， 也 很 可 能 是 第 一 本 以 视觉 SLAM 为 
主题 的 中 文书 。 
那么 ，SLAM 是 什么 ? 


SLAM 是 Simultaneous Localization and Mapping 的 缩写 ， 中 文 译 
作 “ 同 时 定位 与 地 图 构建 N 。 它 是 指 欣 载 特定 传 感 硕 的 主体 ， 在 没有 
环境 先 验 信息 的 情况 下 ， 于 运动 过 程 中 建立 环境 的 模型 ， 同 时 估计 目 
己 的 运动 外 。 如 条 这 里 的 传 感 厚 主要 为 相机 ， 那 承 称 为 “和 视觉 SLAM7”。 

本 书 的 主题 束 是 视觉 SLAM。 这 里 我 们 刻意 把 许多 个 定义 放 到 一 名 
话 中 ， 有 是 希望 访 者 有 一 个 较 明 确 的 概念 。 首 先 ，SLAM 的 目的 是 解 
决 “ 定 位 ”与 “地 图 构建 ”这 两 个 问题 。 也 就 是 说 ， 一 边 要 估计 传 感 右 目 和 刁 
的 位 置 ， 一 边 要 建立 周围 环境 的 模型 。 那 么 怎么 解决 呢 ? 这 需要 用 到 传 
感 左 的 信息 。 传 司 希 能 以 一 定形 陈 观 察 外 部 的 世界 ， 不 过 不 同 传 感 厚 观 
察 的 方式 古人 不同 的 。 而 之 所 以 要 论 一 本 书 的 内 容 去 讨论 这 个 问题 ， 是 因 
为 它 很 难 一 特别 是 我 们 硕 望 实 时 地 、 在 没有 先 验 知识 的 情况 下 进行 
SLAM。 妆 用 相机 作为 传 感 融 时， 我 们 要 做 的 束 是 根据 一 张 张 连续 运动 
的 图 像 《 它 们 形成 了 一 段 视频 ) ， 从 中 推 新 相机 的 运动 ， 以 及 周围 环境 
的 情况 。 

这 似乎 是 个 很 直观 的 问题 。 我 们 目 己 走 进 卫生 的 环境 时 不 就 是 这 人 么 
做 的 吗 ? 

在 计算 机 视 东 “ComputerVision) EJZ 48]. ATTA ZEA HA 
Hi BOUMCNDA—REE, TIER A SSE, FEAR ATEN, PREZ AS 
知 的 领域 一 一 这 是 一 个 美妙 而 又 银 漫 的 梦想 ， 吸 引 了 无 数 的 科研 人 员 日 
BALES 。 我 们 曾经 以 为 这 件 事情 并 不 困难 ， 然 而 进展 却 远 不 如 预 
想 的 那么 顺利 。 我 们 眼中 的 花草 树木 、 虫 鱼 乌 四， 在 计算 机 中 却 是 那样 





的 不 同 : 它们 只 是 一 个 个 由 数字 排列 而 成 的 窍 阵 〈Matrix) 。 让 计算 机 
理解 图 像 的 内 容 ， 就 像 让 我 们 自己 理解 这 些 数字 一 样 困 难 。 我 们 既 不 了 
解 目 己 如 何 理解 图 像 ， 也 个 知道 计算 机 该 如 何 理解 、 探 索 这 个 世界 。 于 
是 我 们 困惑 了 很 人 人 ， 和 直到 几 十 年 后 的 今天 ， 才 友 现 了 一 点 上 成 功 的 迹 
fà: 通过 人 工 智能 (Arti fi cial Intelligence)〉 和 机 器 学 习 (Machine 
Learning) 技术 ， 计 算 机 浙 渐 能 够 辨别 出 物体 、 人 脸 、 声 音 、 文 字 
尽管 它 所 用 的 方式 〈 概 率 学 建 模 ) 与 我 们 是 如 此 不 同 。 男 一 方面 ， 在 
SLAM 人 及 展 了 将 近 30 年 之 后 ， 我 们 的 相机 才 新 渐 开 始 能 够 认识 到 上 自 续 的 
人 位置， 友和 沉 目 己 在 运动 一 一 虽然 方式 还 是 和 我 们 人 类 有 巨大 的 甜 卉 。 不 
过 ， 人 至 少 研究 者 们 已 经 成 功 地 搭建 出 种 种 实时 SLAM 系 统 ， 有 的 能 够 快 
速 跟 踪 自 身 位 置 ， 有 的 甚至 能 够 进行 实时 的 三 维 重 建 。 


这 件 事情 确实 很 困难 ， 但 我 们 已 经 有 了 很 大 的 进展 。 更 令 人 兴奋 的 
是 ， 近 年 来 随 看 科技 的 有 发展， 涌现 出 了 一 大 批 与 SLAM 相 关 的 应 用 点 。 
在 许多 地 方 ， 我 们 都 希望 知道 自身 的 位 置 : 室内 的 扫地 机 和 移动 机 器 人 
需要 定位 ， 野 外 的 目 动 驾驶 汽车 需要 定位 ， 空 中 的 无 人 机 需要 定位 ， 虐 
拟 现实 和 增强 现实 的 设备 也 需要 定位 。SLAM 是 那样 重要 。 没 有 和 它 ， 扫 
地 机 束 无 法 在 房间 自主 地 移动 ， 只 能 盲目 地 游荡 ; 家 用 机 器 人 惑 无 法 按 
照 指 令 准 确 到 达 茶 个 房间 ; 虚拟 现实 也 将 永远 固定 在 座 椅 之 上 一 A 
这 些 新 奇 的 事物 都 无 法 出 现在 现实 生活 中 ， 那 将 多 么 令 人 遗憾 。 


今天 的 研究 者 和 应 用 开发 人 员 ， 逐 渐 意 识 到 了 SLAM 技 术 的 重要 
性 。 在 国际 上 ，SLAM 已 经 有 近 三 十 年 的 研究 历史 ， 也 一 直 是 机 研 人 和 
计算 机 视 党 的 研究 热点 。21 世 纪 以 来 ， 以 视觉 传感器 为 中 心 的 视觉 
SLAM 技 术 ， 在 理论 和 实践 上 都 经 历 了 明显 的 转变 与 突破 ， 正 逐步 从 实 
验 室 研究 迈 问 市 场 应 用 。 同 时 ， 我 们 又 遗憾 地 发 现 ， 至 少 在 国内 ， 与 
SLAM 相 关 的 论文 、 书 籍 仍然 非常 医 乏 ， 让 许多 对 SLAM 拉 术 感 兴趣 的 
初学 者 无 从 一 壬 门 符 。 虽 然 SLAM 的 理论 框架 基本 趋 于 稳定 ， 但 其 编程 
SEU PARA BAR, AG BOR IYO Vii. WIZE ASLAM Sida HY ot A 
A, AATREKBJB, SS AR, EERE RIES SRT 
以 接近 SLAM 技 术 的 核心 。 

本 书 全 面 系统 地 介绍 了 以 视 党 传 感 需 为 主体 的 视觉 SLAM 技 术 ， 我 
们 希望 它 能 (部 分 地 ) 填补 这 方面 资料 的 衬 折 。 我 们 会 详细 地 介绍 
SLAM 的 理论 育 景 、 系 统 巢 构 ， 以 及 各 个 模块 的 主流 做 法 。 同 时 ， 极 其 
重视 实践 ”: 本 书 介 绍 的 所 有 ”重要 算法 ， 都 将 给 出 可 以 运行 的 实际 代 








公 ， 以 求 加深 读 者 的 理解 。 之 所 以 这 么 做 ， 主 要 是 考虑 到 SLAM 毕 竞 是 
一 项 和 实践 紧密 相关 的 技术 。 再 漂亮 的 数学 理论 ， 如 果 不 能 转化 为 可 以 
运行 的 代码 ， 那 就 仍 是 可 望 而 不 可 即 的 空中 楼 阁 ， 没 有 实际 意义 。 我 们 
相信 ， 实 践 出 真知 ， 实 践 出 真爱 。 只 有 实际 地 演算 过 各 种 算法 之 后 ， 你 
才能 真正 地 认识 SLAM， 真 正 地 喜欢 上 科研 。 


自 1986 年 提出 以 来 由 ，SLAM 一 直 是 机 器 人 领域 的 热点 问题 。 关 于 
它 的 文献 数 以 二 计 ， 想 要 对 SLAM 友 展 史 上 的 所 有 算法 及 变种 做 一 个 完 
整 的 说明， 是 十 分 困难 而 且 没 有 必要 的 。 本 书 中 会 介绍 SLAM 所 军 涉 的 
ARAR, PS SU. URDU. Ui iB. SESE Rae 
等 ， 并 在 这 些 背景 知识 之 上 ， 给 出 SLAM 这 村 大 树 的 主干 ， 而 略 去 一 部 
分 形状 奇特 、 纹 理 复 杂 的 枚 时 。 我 们 认为 这 种 做 法 是 有 效 的 。 如 采 放 者 
能 够 掌握 主干 的 精 艇 ， 那 么 目 然 会 有 能 力 去 探索 那些 边缘 的 、 细 和 的 、 
错综复杂 的 前 治 知 识 。 所 以 ， 我 们 的 目的 是 ， 让 SLAM 的 初学 者 通过 疯 
读本 书 快速 地 成 长 为 能 够 探索 这 个 领域 边缘 的 研究 着 。 画 一 方面 ， 即 便 
你 已 经 是 SLAM 领 域 的 研究 人 员 ， 本 书 也 可 能 有 一 些 你 还 和 得 卫生 的 地 
方 ， 可 以 让 你 产生 新 的 见解 。 

目前 ， 与 SLAM 相 天 的 书籍 主要 有 《概率 机 器 人 》 (Probabilistic 
robotics 25!  、《 计 算 机 视觉 中 的 多 视图 几何 》 (Multiple X View 
Geometry in Computer Vision ) B). NLA Z PRIR Sit) (State 
Estimation for Robotics:A Matrix-Lie-Group Approach ) 1 等 。 它 们 内 容 
丰富 、 论 述 人 全面、 推导 严谨 ， 是 SLAM 研 究 者 中 脸 灾 人 口 的 经 典 教 材 。 
然而 束 目 前 来 看 ， 还 存在 两 个 午 要 的 问题 ， 其 一， 这 些 图 书 的 目的 在 于 
介绍 基础 理论 ，SLAM 只 是 其 应 用 之 一 。 因 此 ， 它 们 并 不 能 算是 专门 讲 
解 SLAM 的 书籍 。 其 二 ， 它 们 的 内 容 俩 重 于 数学 理论 ， 基 本 不 涉及 编程 
实现 ， 导 致 读者 经 营 出 现 “ 书 能 看 异 却 不 会 编程 ”的 情况 。 而 我 们 认为 ， 
只 有 读者 杀 目 实现 了 了 算法， 调试 了 各 个 参数 ， 才 能 谈 得 上 真正 理解 了 问 
UA Er. 


我 们 会 提 及 SLAM 的 历史 、 理 论 、 算 法 、 现 状 ， 并 把 完整 的 SLAM 
系统 分 成 几 个 模块 ， 视 知 里 程 计 、 后 新 优化 、 建 图 ， 以 及 回环 检测 。 我 
们 将 陪 着 读者 一 点 点 实现 这 些 模 块 中 的 核心 部 分 ， 探 讨 它们 在 什么 情况 
下 有 效 ， 什 么 情况 下 会 出 问题 ， 并 指导 大 家 在 目 己 的 机 右上 运行 这 些 代 
伺 。 你 会 接触 到 一 些 必 要 的 ”数学 理论 和 许多 编程 知识 ， 会 用 到 Eigen、 


OpenCV, PCL, g20o, Ceres% EN ， 和 营 握 它们 在 Linux 操 作 系 统 中 的 使 
FAAS 

从 写作 风格 上 ， 我 们 不 想 把 本 书写 成 枯燥 的 理论 书籍 。 拉 术 类 图 书 
应 该 是 严 证 可 菲 的 ， 但 严 谍 不 意味 看 刻板 。 一 本 优秀 的 技术 书 应 该 是 生 
动 有 趣 而 易于 理解 的 。 如 采 你 觉得 “这 个 作者 怎么 这 么 不 正经 ?， 敬 请 原 
这 ， 因 为 我 并 不 是 一 个 非常 严肃 的 人 中 。 无 论 如 何 ， 有 一 件 事 是 可 以 上 衣 
FEN: 只 要 你 对 这 门 新 技术 感 兴趣 ， 在 学 习 本 书 的 过 程 中 肯定 会 有 上 所 收 
3k! 您 会 掌握 与 SLAM 相 天 的 理论 知识 ， 你 的 编程 能 力也 将 有 明显 的 进 
步 。 在 很 多 时 候 ， 您 会 有 有 一 种 “我 在 陪 你 一 起 做 科研 ”的 感觉 ， 这 正 是 我 
所 希望 的 。 但 愿 您 能 在 此 过 程 中 发 现 研 究 的 乐趣 ， 政 欢 这 种 “通过 一 香 
努力 ， 看 到 事情 顺利 运行 ?的 成 就 感 。 

好 了 ， 话 不 多 说 ， 祝 你 旅行 愉快 ! 


1.2 ”如 何 使 用 本 书 


1.2.1 组 织 方式 


本 书 名 为 “视觉 SLAM 十 四 讲 ”。 顾 名 思 义 ， 我 们 会 像 在 学 校 里 讲 诛 
那样 ， 以 * 讲 ”作为 本 书 的 基本 单元 。 每 一 讲 都 对 应 一 个 固定 的 主题 ， 其 
中 会 穿插 “理论 部 分 ”" 和 “实践 部 分 ”两 种 内 容 。 通 第 十 理论 部 分 在 有 前， 实 
践 部 分 在 后 。 在 理论 部 分 中 ， 我 们 将 介绍 理解 算法 所 必需 的 数学 知 
识 ， 并 且 大 多 数 时 候 以 叙述 的 方式 ， 而 不 是 像 数 学 教科 书 那 样 用 “定义 
一 定理 一 推论 ”的 方式 ， 因 为 我 们 觉得 这 样 的 方式 阅读 起 来 更 容易 一 
些 ， 尽 党 有 时 候 巡 得 不 那么 严谨 。 实 践 部 分 主要 下 编程 实现 ， 讨 论 程 序 
里 各 部 分 的 含义 及 实验 结果 。 看 到 标题 中 市 有 “实践 ”两 个 字 的 革 市 ， 你 
PMA BEA) 打开 电脑 ， 和 我 们 一 起 愉快 地 编写 代 人 码 了。 

值得 一 所 的 是 ， 我 们 只 会 把 与 解决 问题 相关 的 数学 知识 放 在 书 里 ， 
并 尽量 保持 线 显 。 虽 然 我 们 是 工科 生 ， 但 也 要 承认 ， 茶 些 做 法 只 要 经 验 
上 够 用 ， 设 必要 非得 在 数学 上 奶 求 完备 。 只 要 我 们 知道 这 些 复 法 能 够 工 
作 ， 并 且 数 学 家 们 香 诉 了 我 们 在 什么 情况 下 可 能 不 工作 ， 那 么 我 们 融 表 
示 满 章 ， 而 不 退 究 那些 看 似 完美 但 实际 复 森 见长 的 证 明 (当然 它们 固有 
目 己 的 价值 》。 由 于 SLAM 军 涉 a 到 了 太 多 数学 背景 ， 为 了 防止 使 本 书 变 
成 数学 教科 书 ， 我 们 把 一 些 细节 上 的 推导 和 证 明 留 作 习 题 和 补充 阅读 材 
料 ， 方 便 感 兴趣 的 谈 者 进一步 阅读 参考 文献 ， 更 深入 地 和 苞 握 相关 细 布 。 


每 一 讲 正文 之 后 ， 我 们 设计 了 一 些 习 题 。 其 中 ， 带 * 号 的 习题 是 具 
有 一 定 难 度 的 。 我 们 强烈 建议 读者 把 习题 都 练习 一 裔 ， 这 对 你 掌握 这 些 
知识 很 有 帮助 中 。 

全 书 内 容 主 要 分 为 两 个 部 分 。 

1. 第 一 部 分 为 数学 基础 篇 ， 我 们 会 以 浅显 易 民 的 方式 ， 铺 垫 与 视觉 
SLAM 相 关 的 

数学 知识 ， 包 括 : 

第 1 讲 是 前 言 ， 介 绍 这 本 书 的 基本 人 信息， 习题 部 分 主要 包括 一 些 有 日 


ii 


第 2 讲 为 SLAM 系 统 概 述 ， 介 绍 一 个 SLAM 系 统 由 哪些 模块 组 成 ， 
各 模块 的 具体 工作 是 什么 。 实 践 部 分 介绍 编程 环境 的 搭建 过 程 以 及 IDE 
的 使 用 。 


第 3 讲 介绍 三 维 空 间 运 动 ， 你 将 接触 到 旋转 矩阵 、 四 元 数 、 欧 拉 角 
的 相关 知识 ， 并 且 在 Eigen 当 中 使 用 它们 。 


“第 4 讲 为 李 群 和 李 人 代数。 即便 你 现在 不 恒 李 代数 为 何 物 ， 也 没有 天 
系 。 你 将 学 到 李 代 数 的 定义 和 使 用 方式 ， 然 后 通过 Sophus 操 作 它 们 。 


“第 5 讲 介 绍 针 孔 相 机 模型 以 及 图 像 在 计算 机 中 的 表达 。 你 将 用 
OpenCV 来 调 取 相 机 的 内 外 参数 。 


“第 6 讲 介绍 非 线 性 优化 ， 包 括 状态 估计 理论 基础 、 节 小 二 乘 问题 、 
榜 度 下 降 方法 。 你 会 完成 一 个 使 用 Ceres 和 8g&2o 进 行 曲 线 拟 合 的 实验 。 


这 些 就 是 我 们 要 用 到 的 所 有 数学 知识 了 ， 当 然 ， 其 中 还 隐 含 了 你 以 
前 学 过 的 高 等 数学 和 线性 代数 。 我 们 保证 它们 看 起 来 都 不 会 很 难 。 当 
然 ， 奋 你 想 进 一 步 深入 控 据 ， 我 们 会 提供 一 些 参考 资料 供 你 阅读， 那些 
材料 可 能 会 比 正文 里 讲 的 知识 难 一 些 。 

2. 第 二 部 分 为 SLAM 技 术 篇 。 我 们 会 使 用 第 一 部 分 所 介绍 的 理论 ， 
讲述 视觉 SLAM 中 各 个 模块 的 工作 原理 。 

。 第 7 讲 为 特征 点 法 的 视觉 里 程 计 。 该 讲 内 容 比较 多 ， 包 括 特 征 点 的 
提取 与 匹配 、 对 极 几 何 约束 的 计算 、PnP 和 ICP 等 。 在 实践 中 ， 你 将 用 这 
些 方法 去 估计 两 个 图 像 之 间 的 运动 。 

第 8 讲 为 直接 法 的 视觉 里 程 计 。 你 将 学 习 光 流 和 直接 法 的 原理 ， 然 
后 利用 g2o 实 现 一 个 简单 的 RGB-D 有 直接 法 。 

。 第 9 讲 为 视觉 里 程 计 的 实践 章 ， 你 将 搭建 一 个 视觉 里 程 计 框架 ， 综 
合 运 用 先前 学 过 的 知识 ， 实 现 它 的 基本 功能 。 这 个 过 程 中 ， 你 会 磁 到 一 
些 问 题 ， 例 如 优化 的 必要 性 、 关 键 帧 的 选择 等 。 

第 10 讲 为 后 端 优 化 ， 主 要 为 对 Bundle Adjustment 的 深入 讨论 ， 包 括 
基本 的 BA， 以 及 如 何 利 用 稀 朴 性 加 速 求解 过 程 。 你 将 用 Ceres 和 g2o 分 别 
书写 一 个 BA 程序 。 


第 11 讲 主要 讲 后 端 优 化 中 的 位 姿 图 。 位 姿 图 是 表达 关键 帆 之 间 约 
训 的 一 种 更 紧凑 的 形式 。 你 将 用 g2o 和 gtsam 对 一 个 位 姿 球 进行 优化 。 

第 12 讲 为 回环 检测 ， 主 要 介绍 以 词 袋 方法 为 主 的 回环 检测 。 你 将 
使 用 dbow3 书 写字 典 训练 程序 和 回环 检测 程序 。 

。 第 13 讲 为 地 图 构建 。 我 们 会 讨论 如 何 使 用 单 目 进行 稠密 深度 图 的 
估计 【以 及 这 是 多 么 不 可 靠 ) ， 然 后 讨论 RGB-D 的 稠密 地 图 构建 过 程 。 
你 会 书写 极 线 搜索 与 块 匹 配 的 程序 ， 然 后 在 RGB-D 中 遇 到 点 云 地 图 和 八 
又 树 地 图 的 构建 问题 。 

第 14 讲 主要 介绍 当前 的 开源 SLAM 项 目 以 及 未 来 的 发 展 方向 。 相 信 
在 阅读 了 前面 的 知识 之 后 ， 你 会 更 容易 理解 它们 的 原理 ， 实 现 目 己 的 新 
想法 。 

最 后 ， 如 果 你 完全 看 不 懂 上 面 在 说 什么 ， 那 么 茶 喜 你 ! 这 本 书 很 适 
合 你 ! 加 油 ! 


1.2.2 RAS 


APAA UM IS tte E github E: 
https://github.com/gaoxiang12/slambook 


蝇 烈 建议 谈 者 下 载 它们 以 供 随 时 奏 看 。 代 但 是 投 章节 划分 的 ， 比 
如 ， 第 7 讲 的 内 容 束 会 放 在 ch7 文 件 夹 中 。 此 外 ， 对 于 书 中 用 到 的 一 些小 
型 库 ， 会 以 压缩 包 的 形式 放 在 3rdparty 文 件 夹 下。 对 于 像 OpenCV 那 种 大 
中 型 库 ， 我 们 会 在 它们 第 一 次 出 现时 介绍 其 安装 方法 。 如 果 你 对 代码 有 
任何 疑问 ， 请 单 击 GitHub 上 的 Issues 按 钮 ， 提 交 间 题 。 如 果 确 实 是 代码 
出 现 问 题 ， 我 们 会 及 时 进行 修改 ;， 即 使 是 你 的 理解 有 信 差 ， 我 们 也 会 尽 
可 能 回复 。 如 果 你 不 习惯 使 用 Git， 那 么 单 击 右 侧 包 含 download 字 样 的 
按钮 将 代码 下 载 公 本 地 即 可 。 


1.2.3 rl I] HJ d 


本 书面 向 对 SLAM 感 兴趣 的 学 生 和 研究 人 员 。 阅 读本 书 需要 一 定 的 


基础 ， 我 们 假设 你 具备 以 下 知识 : 

“高 等 数学 、 线 性 代数 、 概 率 论 。 这 些 是 大 部 分 读者 应 该 在 大 学 本 
科 阶 段 接 触 过 的 基本 数 季 知识。 你 应 当 明 日 矩阵 和 问 量 是 什么 ， 或 者 做 
微分 和 积分 是 什么 意思 。 对 于 SLAM 中 用 到 的 专业 知识 ， 我 们 会 额外 加 
以 介绍 。 


“C++ 语言 基础 ”。 由 于 我 们 采用 C++ 作为 编码 语言 ， 所 以 建议 读者 
至 少 熟 入 这 门 语言 有 的 语法 。 比 如 ， 你 应 该 知道 类 是 什么 ， 如 何 使 用 
C++ 标准 库 ， 模 板 英 如 何 使 用 ， 等 等 。 我 们 会 避免 过 多 地 使 用 技巧 ， 但 
有 些 地 方 确实 无 法 避免 。 此 外 ， 我 们 还 使 用 了 一 些 C++11 标 准 的 内 容 ， 
不 过 ， 我 们 会 在 用 到 的 地 方 加 以 解释 。 


"Linux 基 础 ”。 我 们 的 开 肥 环境 是 Linux 而 非 wWindows， 并 且 只 提供 
Linux 下 的 源 程 序 ， 不 会 再 提供 Windows 下 的 开发 方法 介绍 。 我 们 认 
AY, 42 Linux 是 一 个 SLAM 研 究 人 员 所 必需 的 ， 请 初学 者 暂时 不 要 问 
为 什么 ， 把 本 书 的 知识 学 好 之 后 相信 你 会 和 我 们 有 同样 的 想法 。 各 种 
程序 库 在 Linux 下 的 配置 都 非常 便捷 ， 你 也 会 在 此 过 程 中 体会 到 Linux 的 
便利 。 如 果 读 者 此 前 从 未 使 用 过 Linux， 那 么 最 好 找 一 本 Linux 的 教材 稍 
加 学 习 【〈 尚 握 基 本 知识 即 可 ， 一 般 束 是 相关 图 书 的 前 面 几 章 内容 〉 。 我 
们 不 要 求 谈 者 具备 高 超 的 Linux 操 作 撤 能， 但 希望 谈 者 全 少 知 道 “ 打 开 终 
痢 ， 进 入 代码 目录 ”是 如 何 操 作 的 。 本 讲 的 习题 里 有 一 些 Linux 知 识 目 测 
了 页， 如 果 你 清楚 目测 题 的 管 案 ， 那 么 阅读 本 书 代 人 码 不 会 有 任何 问题 。 


对 SLAM 感 兴趣 但 不 具备 上 述 知 识 的 读者 ， 可 能 在 阅读 本 书 时 会 感 
到 困难 。 如 果 你 不 了 解 C++ 的 基本 知识 ， 可 以 读 一 点 C++Primer Plus 之 
类 的 图 书 入 门 ; 如 果 你 缺少 相关 的 数学 知识 ， 也 可 以 先 疯 谈 一 些 相关 数 
学 教材 补充 知识 ， 不 过 我 们 认为 ， 对 大 多 数 大 学 本 科 水 平 的 朋友 ， 读 恒 
本 书 所 需 的 数学 背景 表 定 是 具备 了 了 。 人 代码 方 面 ， 你 最 好 花 点 时 间 杀 目 输 
入 一 过 ， 再 调节 里 面 的 参数 ， 看 看 效果 会 发 生 怎 样 的 改变 。 这 会 对 学 习 
很 有 帮助 。 

本 书 可 作为 SLAM 相 关 课 程 的 教材 ， 亦 可 作为 课外 目 学 材料 使 用 。 


1.3 RAE 


本 书 既 有 数学 理论 介绍 ， 也 有 编程 实现 ， 因 此 ， 为 方便 阅读 ， 对 不 
同 内 容 采 用 了 不 同 排版 方式 加 以 区 分 。 

1. 数 学 公 陈 单独 列 出 ， 重 要 的 公 云 还 在 右 侧 标 了 序号 ， 例 如 : 

y = Ax. (1.1) 

Pree EFA R AS Colla ) ， 癌 量 和 和 矩阵 使 用 粗 斜 体 ( 如 qa,A ) . F 
心 粗 体 代 表 特 殊 集 合 ， 如 实数 集 R、 整 数 集 Z。 李 代数 部 分 使 用 哥 特 
体 ， 如 se(3)。 

2. 程 序 代 人 码 以 方 框框 出 ， 使 用 小 一 些 的 字 写 ， 左 侧 帘 有 行 写 。 如 果 
程序 较 长 ， 方 框 会 延续 到 下 一 页 : 


1 | #include <iostream> 


2 |using namespace std; 






4 |int main ( int argc, char** argv ) 


cout««"Hello"««endl; 


return 0; 


7 
8 |} 


3. 当 代码 数量 较 多 或 有 的 部 分 与 之 前 列 出 的 重复 ， 不 适合 完全 列 在 
书 中 时 ， 我 们 会 仅 给 出 重要 片段 ， 并 以 “片段 ”二 字 注 明 。 因 此 ， 我 们 
强烈 建议 读者 到 GitHub 上 下 载 所 有 源 代码 ， 完 成 练习 ， 以 更 好 地 掌握 本 
书 知识 。 

4. 由 于 排版 原因 ， 书 中 展示 的 代码 可 能 与 GitHub 中 的 代码 有 稍 许 不 
同 ， 请 以 GitHub 上 的 代码 为 准 。 

5. 我 们 用 到 的 每 个 库 ， 在 第 一 次 出 现 的 时 候 会 有 比较 详细 的 说 明 ， 
但 在 后 续 的 使 用 中 则 不 再 歼 述 。 所 以 ， 建 议 读 者 按 章 节 顺 序 阅 读本 书 内 
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6. 每 一 讲 的 开头 会 列 出 本 讲 的 内 容 所 要， 而 末尾 会 有 小 结 和 练习 
题 。 引 用 的 参考 文献 在 全 书 末 尾 列 出 。 

7. 以 星 写 开头 的 章节 是 选读 部 分 ， 读 者 可 以 根据 兴趣 阅读 。 跳 过 它 
们 不 会 对 理解 后 续 革 市 产生 影响。 

8. 文 中 重要 的 内 容 以 黑体 标 出 ， 相 信和 你 已 经 习惯 了 。 

9. 我 们 设计 的 实验 大 多 数 是 针 示 性 质 的 。 看 情 了 它们 不 代表 你 已 经 
然 悉 整个 库 的 使 用 。 所 以 我 们 建议 你 在 读 外 人 花 一 点 时 间 ， 对 本 书 经 闻 用 
的 几 个 库 进 行 深入 学 习 。 

10. 本 书 的 习题 和 选读 内 容 可 能 需要 你 目 己 搜索 额外 材料 ， 所 以 你 
项 要 学 会 使 用 搜索 引擎 。 


1.4 SEWER E HÀ 
在 本 书 漫 长 的 写作 过 程 中 ， 我 们 得 到 了 许多 人 的 帮助 ， 包 括 但 不 限 


“中科院 的 贺 一 家 博士 为 第 5 讲 的 相机 模型 部 分 提供 了 材料 。 

* 闫 沁 窒 提供 了 第 7 讲 的 公式 推导 材料 。 

“华中 科大 的 刘 妆 博士 为 本 书 第 6 讲 和 人 第 10 讲 提供 了 材料 。 

* 估 多 的 老师 、 同 学 为 本 书 提供 了 修改 意见 : KAR UNE GES Fk 
Hk. EIR, x oR. xi. PAA, RRA. ER, ARX. TX 
ZR. WTR. MEW. mue. ZA. RE. SSO. okies. ASI. p 
Xu. M. SER. FELLA Ie RW e 

此 外 ， 感 谢 我 的 导师 张涛 教授 一 直 以 来 对 我 的 支持 和 帮助 。 感 谢 电 
子 工 业 出 版 社 郑 柳 洁 编辑 的 支持 。 没 有 他 们 的 帮助 ， 本 书 不 可 能 以 现在 
的 面貌 来 到 读者 面前 。 本 书 的 成 书 与 出 版 是 所 有 人 共同 努力 的 结晶 ， 尺 
官 我 没 法 把 他 们 都 列 在 作者 列表 中 ， 但 是 它 的 出 版 离 不 开 他 们 的 工作 。 

本 书写 作 过 程 中 参考 了 大 量 文献 和 论文 。 其 中 大 部 分 数学 理论 知识 
是 前 人 研究 的 成 果 ， 并 非 我 的 原创 。 一 小 部 分 实验 设计 亦 来 自 各 开源 代 
码 的 演示 程序 ， 不 过 大 部 分 是 我 自己 编写 的 。 此 外 ， 也 有 一 些 图 片 摘自 
公开 发 表 的 期 刊 或 会 议论 文 ， 文 中 均 已 注 明 。 未 做 说 明 的 图 像 ， 或 为 原 
创 ， 或 来 日 网 络 ， 息 不 一 一 列举 。 如 有 问题 ， 请 与 我 们 联系 ， 我 们 会 在 
第 一 时 间 加 以 修正 。 


本 书 涉 及 知识 点 众多 ， 错 漏 在 所 难免 。 如 有 疑问 ， 欢 迎 通 过 电子 邮 
件 与 我 联系 。 


我 的 邮箱 是 : gaoxiang12@mails.tsinghua.edu.cn。 

感谢 我 的 爱人 刘 丽 连 女 士 长 期 的 理解 和 支持 。 这 本 书 是 献 给 她 的 。 
习题 〈 基 本 知识 目测 题 ) 

1. 有 线性 方程 Ax =b , Æ ORIA, b ， 需 要 求解 x ， 该 如 何 求解 ? 这 对 


A 和 有 哪些 要 求 ? 提示 : 从 A 的 维度 和 秩 角 上 度 来 分 析 。 


2. 局 斯 分 布 是 什么 ? 它 的 一 维 形式 是 什么 样子 ? EM AE SEIT 
么 样子 ? 


3. 你 知道 C++ 中 的 类 吗 ? PRABBSTLRA? 你 使 用 过 它们 吗 ? 


4. 你 以 前 怎样 书写 C++ 程 序 ? (你 完全 可 以 说 只 在 Visual C++6.0 下 
写 过 C++ 工 程 ， 只 要 你 有 写 C++ 和 C 语 言 的 经 验 就 行 。) 


5. 你 知道 C++11 标 准 吗 ? 其 中 哪些 新 特性 你 听 说 过 或 用 过 ? 有 没有 
其 他 的 标准 ? 


6. 你 知道 Linux 吗 ? 你 有 没有 有 至少 使 用 过 一 种 《不 算 安 里) ， 比 如 
Ubuntu? 


7.Linux 的 目录 结构 是 什么 样 的 ? 你 知道 哪些 基本 命令 ， 比 如 ls,cat 
等 ? 


8. 如 何在 Ubuntu 中 安装 软件 (不 打开 软件 中 心 的 情况 下 〉? 这 些 软 
件 被 安装 在 什么 地 方 ?” 如 果 只 知道 模糊 的 软件 名 称 〈 比 如 想 要 装 一 个 名 
称 中 含有 eigen 的 库 ) ， 应 该 如 何 安 装 它 ? 


9.* 花 一 个 小 时 学 习 一 下 Vim， 因 为 你 迟早 会 用 它 。 你 可 以 在 终端 中 
输入 vimtutor 疯 旋 一 过 所 有 内 容 。 我 们 不 需要 你 非常 熟练 地 操作 它 ， 只 
要 能 够 在 学 习 本 书 的 过 程 中 使 用 它 输入 代码 即 可 。 不 要 在 它 的 插件 上 浪 
费时 间 ， 不 要 想 着 把 Vim 用 成 IDE， 我 们 只 用 它 做 文本 编辑 的 工作 。 








[1] 如 末 你 完全 没有 听 说 过 它们 ， 那 么 应 该 感到 兴奋 ， 这 说 明 你 会 从 本 书 中 收获 很 多 知识 。 
[2] 你 会 经 党 在 脚注 中 发 现 一 些 神奇 的 东西 。 
[3] 它们 也 可 能 成 为 今后 相关 行业 的 面试 题 ， 或 许 还 能 帮 你 在 找 工作 时 留 个 好 印象 。 








第 2 讲 ” 初 识 SLAM 


主要 目标 
1. 理 解 一 个 视 党 SLAM 框 架 由 哪儿 个 模 顽 组 成 ， 各 模块 的 任务 是 什 


Ss 


2. 搭 建 编 程 环境 ， 为 开发 和 实验 做 准备 。 

3. 理 解 如 何在 Linux 下 编译 并 运行 一 个 程序 ， 如 果 程 序 出 了 问题 ， 叉 
该 如 何 对 它 进 行 调试 。 

4. 和 掌握 cmake 的 基本 使 用 方法 。 


本 讲 概括 地 介绍 一 个 视觉 SLAM 系 统 的 结构 ， 作 为 后 续 内 容 的 大 
纲 。 实 践 部 分 介绍 环境 搭建 、 程 序 基 本 知识 ， 最 后 完成 一 个 “Hello 
SLAM”* 程 序 。 
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2.1 引子 : DBNBS 


假设 我 们 组 痊 了 一 台 叫 作 “ 小 画 卜 ”的 机 旧 人 ， 大 概 的 样子 如 图 2-1 
Bran o 


发 射 信息 的 天 线 


a 


F ( 装饰 品 ) 
( 另 一 边 也 是 ) 
笔记 本 存放 处 [一 | 
IT 


一 组 轮子 
图 2-1 NS MEI. 左边: en MAR: 设备 有 相机 、 轮 子 、 笔 记 本 ， 手 

虽然 有 氮 像 *“ 安 早 ”， 但 它 并 不 是 靠 安 里 系统 来 计算 的 。 我 们 把 一 全 
笔记 本 守 进 了 它 的 后 备 箱 内 《方便 我 们 随时 拿 出 来 调试 程序 ) 。 它 能 做 
FT AE? 

RIETEN EREA BEEJ EI . BOAT EWA BOER 
SEVERE Mla A, REEMA Diu RBCS OR ANE BP ae ERA 
可 以 胜任 这 些 事情 。 作 为 机 融 人 ， 我 们 希望 小 区 小 能 够 在 房间 里 上 自由 移 
Bo AN ERATE RR, Ea TH IE OK 

BAB ON cts A Fe FAL, BrESTTOEANEE PEL RO Boe SET 
CETL a A AIR EAS, REANA) . ASH, Hlas AGULBÉ 
mM Ton I. (ANI, Ae b ANAT At, WR Be 
APAL, ERTED PE Fee Metin She 7g [tUi TS OU A A 





生 ， 我 们 在 它 的 脑袋 上 安 痕 了 一 个 相机 。 安 放 相机 的 主要 动机 ， 和 是 考 店 
到 这 样 一 个 机 如 人 和 人 类 非常 相似 从 画面 上 一 眼 吏 能 看 出 。 有 了 眼 
Ha. AAA DURA ASS, Bee TEER A Ee A EMMITT HE. TRE, R 
f] (AAU "ESL SS AIRE SE IES A SED ARBIRE 
个 房间 ， 它 全 少 需 要 知道 两 件 事 : 


1. 我 在 什么 地 方 ? 定位 。 
2. 周 围 坏 境 十 什么 样 ? 建 图 。 


“定位 "和 “ 建 图 "， 可 以 看 成 感知 的 “内 外 之 分 "。 作 为 一 个 “内 外 莱 
修 "的 小 萝卜 ， 一 方面 要 明白 自身 的 状态 《即位 置 ) ， 另 一 方面 也 要 了 
解 外 在 的 环境 CRD 。 当 然 ， 解 决 这 两 个 问题 的 方法 非常 多 。 比 
方 说 ， 我 们 可 以 在 房间 地 板 上 铺设 导 引 线 ， 在 墙壁 上 贴 识 别 二 维 码 ， 在 
桌子 上 放置 无 线 电 定位 设备 。 如 果 在 室外 ， 还 可 以 在 小 萝卜 脑袋 上 安装 
定位 设备 〈 像 手机 或 汽车 一 样 ) 。 有 了 这 些 东 西 之 后 ， 定 位 问题 是 否 已 
经 解决 了 呢 ? 我 们 不 妨 把 这 些 传感器 〈 见 图 2-2) 分 为 两 类 。 

一 类 传感器 是 携带 于 机 器 人 本 体 上 ”的 ， 例 如 机 器 人 的 轮 式 编码 
器 、 相 机 、 激 光 传感器 ， 等 等 。 另 一 类 是 安装 于 环境 中 的， 例如 前 面 
讲 的 导轨 、 二 维 码 标志 ， 等 等 。 安 装 于 环境 中 的 传 感 设备 ， 通 常 能 够 直 
接 测量 到 机 器 人 的 位 置信 息 ， 简 单 有 效 地 解决 定位 问题 。 然 而 ， 由 于 它 
们 必须 在 环境 中 设置 ， 在 一 定 程度 上 限制 了 机 器 人 的 使 用 范围 。 比 方 
说 ， 有 些 地 方 没有 GPS 信号 ， 有 些 地 方 无 法 铺设 导轨 ， 这 时 该 怎么 做 定 
位 呢 ? 














(b) (c) (f 


图 2-2 一 些 传 感 融 。 (a) 利用 二 维 码 进 行 定位 的 增强 现实 软件 ， (b) GPS 定位 装置 
(c) 铺设 导轨 的 小 车 ， (D OGRA; Ce) IMU 单 元 (Ð 双 目 相机 。 


我 们 看 到 ， 这 类 传感器 约束 了 外 部 环境 。 只 有 在 这 些 约束 满足 
时 ， 基 于 它们 的 定位 方案 才能 工作 。 反 之 ， 当 约束 无 法 满足 时 ， 我 们 就 
没 法 进行 定位 了 。 所 以 说 ， 虽 然 这 类 传 感 磺 简单 可 靠 ， 但 它们 无 法 提供 
一 个 普遍 的 、 通 用 的 解决 方案 。 相 对 地 ， 那 些 携带 于 机 器 人 本 体 上 的 传 
感 器 ， 比 如 激光 传感器 、 相 机 、 轮 式 编 码 器 、 惯 性 测量 单元 (Inertial 
Measurement Unit, IMU) 每 ， 它 们 测 到 的 通常 都 是 一 些 间接 的 物理 量 
而 不 是 直接 的 位 置 数 据 。 例 如 ， 轮 式 编码 器 会 测 到 轮子 转动 的 角度 ， 
IMU 测 量 运 动 的 角速度 和 加 速度 ， 相 机 和 激光 传感器 则 读 取 外 部 环境 的 
某 种 观测 数据 。 我 们 只 能 通过 一 些 间接 的 手段 ， 从 这 些 数 据 推 算 上 自己 的 
人 位置。 虽然 这 听 上 去 是 一 种 迁 回 战术 ， 但 更 明显 的 好 处 是 ， 它 没有 对 环 
境 提出 任何 要 求 ， 从 而 使 得 这 种 定位 方案 可 适用 于 未 知 环境 。 

回顾 前 面 讨论 过 的 SLAM 定 义 ， 我 们 在 SLAM 中 非常 强调 未 知 环 
境 。 在 理论 上 上， 我们 没 法 限制 小 蔓 让 的 使 用 环境 说 ， 这 意味 着 我 们 没 法 
假设 像 GPS 这 些 外 部 传感器 都 能 顺利 工作 。 因 此 ， 使 用 携带 式 的 传 感 右 
来 完成 SLAM 是 我 们 重点 关心 的 问题 。 特 别 地 ， 当 谈论 视觉 SLAM 时 ， 
我 们 主要 是 指 如 何 用 相机 解决 定位 和 建 图 问题 。 

视觉 SLAM 是 本 书 的 主题 ， 所 以 我 们 尤其 关心 小 葛 直 的 眼睛 能 够 做 
些 什么 事 。SLAM 中 使 用 的 相机 与 我 们 平时 见 到 的 单反 摄像 头 并 不 是 同 


一 个 东西 。 它 往往 更 加 简单 ， 不 携 市 是 贯 的 镜头 ， 而 是 以 一 定 速率 提 援 
周围 的 环境 ， 形 成 一 个 连续 的 视频 流 。 普 通 的 摄像 头 能 以 每 秒 钟 30 张 图 
片 的 速度 采集 图 像 ， 高 速 相 机 则 更 快 一 些 。 按 照 工作 方式 的 不 同 ， 相 机 
可 以 分 为 单 目 相 机 (Monocular) 、 双 目 相 机 (Stereo) 和 深度 相机 
(RGB-D) 三 大 类 ， 如 图 2-3 所 示 。 直 观看 来 ， 单 目 相 机 只 有 一 个 摄像 
头 ， 双 目 有 两 个 ， 而 RGB-D 原 理 较 复杂 ， 除 了 能 够 采集 到 彩色 图 睛 之 
外 ， 还 能 读 出 每 个 像 兹 与 相机 之 间 的 距离 。 深 度 相 机 通 第 携 市 多 个 摄像 
站， 工作 原理 和 普通 相机 不 尽 相 同 ， 在 第 5 讲 会 详细 介绍 其 工作 原理 ， 
此 处 读者 只 需 有 一 个 直观 概念 即 可 。 此 外 ，SLAM 中 还 有 全 景 相机 ~ 
Event 相 机 中 等 特殊 或 狐 兴 的 种 类 。 昌 然 伪 尔 能 看 到 它们 在 SLAM 中 的 应 
用 ， 不 过 到 目前 为 止 还 没有 成 为 主流 。 从 样子 上 看 ， 小 区 小 使 用 的 似乎 
AE X H FAAL?! o 





单 目 相机 


双 目 相机 
图 2-3 ”形形色色 的 相机 : 单 目 、 双 目 和 深度 相机 。 


我 们 来 分 别 看 一 看 各 种 相机 用 来 做 SLAM 时 有 什么 特点 。 

单 目 相机 

只 使 用 一 个 摄像 头 进行 SLAM 的 做 法 称 为 单 目 SLAM (Monocular 
SLAM) 。 这 种 传 感 亏 结构 特别 简单 ， 成 本 特别 低 ， 所 以 单 目 SLAM 非 
稼 受 研 究 者 关注 。 你 肯定 见 过 单 目 相机 的 数据 : 照片 。 是 的 ， 作 为 一 张 
Heer, EATARRA? 

照片 本 质 上 是 拍照 时 的 场景 (Scene) 在 相机 的 成 像 平 面 上 留 下 的 
一 个 投影 。 它 以 二 维 的 形式 反映 了 三 维 的 世界 。 显然 ， 这 个 过 程 丢 挥 


了 场景 的 一 个 维度 ， 也 惑 是 所 谓 的 深度 《或 距离 ) 。 在 单 目 相 机 中 ， 我 
们 无 法 通过 单 张 图 片 来 计算 场景 中 物体 与 我 们 之 间 的 距离 OWE) 。 
之 后 我 们 会 看 到 ， 这 个 距离 将 是 SLAM 中 非常 关键 的 信息 。 由 于 我 们 人 
类 见 过 大 量 的 图 像 ， 形 成 了 一 种 天 生 的 二 党， 对 大 部 分 场景 部 有 一 个 生 
观 的 距离 感 〈“ 空 间 感 ) ， 它 可 以 帮助 我 们 判断 图 像 中 物体 的 远近 关 
系 。 比 如 说 ， 我 们 能 够 装 认 出 图 像 中 的 物体 ， 并 且 知 道 其 大 任 的 大 小 ; 

比如 ， 近 处 的 物体 会 挡住 远 处 的 物体 ， 而 太阳 、 月 腕 等 天 体 一 般 在 很 远 
的 地 方 ， 再 如 ， 物 体 受 光照 后 会 留 下 有 影子， 等 等 。 这 些 信息 邦 可 以 帮助 
我 们 判断 物体 的 远近 ， 但 也 存在 一 些 情况 会 使 这 种 距离 感 失效 ， 这 时 我 
们 或 无 法 判断 物体 的 远近 及 其 真实 大 小 了 。 图 2-4 所 示 束 是 这 样 一 个 例 

子 。 在 这 张 图 像 中 ， 我 们 无 法 仅 通 过 它 来 判断 后 面 那 些小 人 是 真实 的 

人 ， 还 是 小 型 模型 。 除 非 我 们 转换 视角 ， 观 察 场景 的 三 维 结构 。 换 襄 

之 ， 在 曲张 图 像 里 ， 你 无 法 确定 一 个 物体 的 真实 大 小 。 它 可 能 是 一 个 很 
大 但 很 远 的 物体 ， 也 可 能 是 一 个 很 近 但 很 小 的 物体 。 由 于 近 大 远 小 的 原 
内 ， 它 们 可 能 在 图 像 中 变 成 同样 大 小 的 样子 。 





e 
图 2-4 ” 单 目 视觉 中 的 尴 众 ;不 知道 深度 时 ， 手 堂上 的 人 是 真人 还 是 模型 ? 


由 于 单 目 相机 拍摄 的 图 像 只 是 三 维 空间 的 二 维 投 影 ， 所 以 ， 如 果真 
想 恢 复 三 维 结构 ， 必 须 改变 相机 的 视角 。 在 单 目 SLAM 中 也 是 同样 的 原 
理 。 我 们 必须 移动 相机 ， 才 能 估计 它 的 运动 (Motion . ， 同 时 估计 场 
景 中 物体 的 远近 和 大 小 ， 不 妨 称 之 为 结构 〈Structure) . MA, EA 
计 这 些 运动 和 结构 呢 ?” 从 生活 经 验 中 我 们 知道 ， 如 果 相 机 往 右 移动 ， 那 


入 图 像 里 的 东西 就 会 往 左边 移动 _ 这 就 给 我 们 推测 运动 带 来 了 信息 。 
另 一 方面 ， 我 们 还 知道 ， 近 处 的 物体 移动 快 ， 远 处 的 物体 则 运动 缓慢 
,于 是 ， 当 相机 移动 时 ， 这 些 物 体 在 图 像 上 的 运动 就 形成 了 视差 。 通 
过 视差 ， 我 们 就 能 定量 地 判断 哪些 物体 离 得 远 ， 哪 些 物体 离 得 近 。 
然而 ， 即 使 我 们 知道 了 物体 远近 ， 它 们 仍然 只 是 一 个 相对 的 值 。 比 
如 我 们 在 看 电影 时 ， 虽 然 能 够 知道 电影 场景 中 哪些 物体 比 另 一 些 大 ， 但 
无 法 确定 电影 里 那些 物体 的 “真实 尺度 *: 那些 大 楼 是 真实 的 高 楼 大 厦 ， 
还 是 放 在 桌 上 的 模型 ? 而 摧毁 大 厦 的 是 真实 怪兽 ， 还 是 穿着 特 摄 服装 的 
演员 ? 直观 地 说 ， 如 果 把 相机 的 运动 和 场景 大 小 同时 放大 两 倍 ， 单 目 相 
机 所 看 到 的 像 是 一 样 的 。 同 样 地 ， 把 这 个 大 小 乘 以 任意 倍数 ， 我 们 都 将 
看 到 一 样 的 景象 。 这 说 明 ， 单 目 SLAM 估 计 的 轨迹 和 地 图 将 与 真实 的 轨 
迹 和 地 图 相差 一 个 因子 ， 也 就 是 所 谓 的 尺度 (Scale) ©. FAKE 
SLAM 无 法 仅 凭 图像 确定 这 个 真实 尺度 ， 所 以 又 称 为 尺度 不 确定 性 。 
平移 之 后 才能 计算 深度 ， 以 及 无 法 确定 真实 尺度 ， 这 两 件 事情 给 单 
目 SLAM 的 应 用 造成 了 很 大 的 麻烦 。 其 根本 原因 是 通过 单 张 图 像 无 法 确 
定 深度 。 所 以 ， 为 了 得 到 这 个 深度 ， 人 们 开始 使 用 双 目 和 深度 相机 。 
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使 用 双 目 相机 和 深度 相机 的 目的 ， 在 于 通过 茶 种 手段 测量 物体 与 我 
们 之 间 的 距离 ， 死 服 单 目 相机 无 法 知道 距离 的 缺点 。 一 旦 知道 了 距离， 
场景 的 三 维 结 构 束 可 以 通过 时 个 图 像 恢 复出 来 ， 也 就 消除 了 尺度 不 确定 
性 。 尽 党 都 是 为 了 测量 距离 ， 但 双 目 相机 与 深度 相机 测量 深度 的 原理 是 
不 一 样 的 。 双 目 相 机 由 两 个 单 目 相机 组 成 ， 但 这 两 个 相机 之 间 的 距离 
( 称 为 基线 (Baseline) ) 是 已 知 的 。 我 们 通过 这 个 基线 来 估计 每 个 像 
系 的 空间 位 置 这 和 人 有 眼 非常 相似。 我 们 人 类 可 以 通过 左右 眼 图 像 的 
差异 判断 物体 的 远近 ， 在 计算 机 上 也 是 同样 的 道理 〈 见 图 2-5) o WR 
对 双 目 相机 进行 拓展 ， 也 可 以 搭建 多 目 相 机 ， 不 过 本 质 上 并 没有 什么 不 
[E]. 











图 2-5” 双 目 相机 的 数据 ， 左 眼 图 像 ， 右 眼 图 像 。 通 过 左右 眼 的 差异 ， 能 够 判断 场景 中 物 
体 与 相机 之 间 的 距离 。 


计算 机 上 的 双 目 相机 需要 大 量 的 计算 才能 (不 太 可 靠 地 ) 估计 每 一 
个 像 系 点 的 深度 ， 相 比 于 人 类 真是 非常 条 拙 。 双 目 相 机 测量 到 的 深度 光 
肋 与 基线 相关 。 基 线 距 离 越 大 ， 能 够 测量 到 的 束 越 远 ， 所 以 无 人 车 上 搭 
载 的 双 目 通常 会 是 个 很 大 的 家 伙 。 双 目 相 机 的 距离 估计 是 比较 左右 眼 的 
图 像 获 得 的 ， 并 不 依赖 其 他 传 感 设 备 ， 所 以 它 既 可 以 应 用 在 室内 ， 亦 可 
应 用 于 室外 。 双 目 或 多 目 相 机 的 缺点 是 配置 与 标定 均 较 为 复杂 ， 其 深度 
量程 和 精度 受 双 目的 基线 与 分 辨 挛 所 限 ， 而 且 视 和 兰 的 计算 非 间 消耗 计算 
资源 ， 需 要 使 用 GPU 和 FPGA 设 备 加 速 后 ， 才 能 实时 输出 整 张 图 像 的 中 
离 信 息 。 因 此 在 现 有 的 条 件 下 ， 计 算 量 是 双 目 的 主要 问题 之 一 。 


深度 相机 《又 称 RGB-D 相 机 ， 在 本 书 中 主要 使 用 RGB-D 这 个 名 

MO 是 2010 年 左右 开始 兴起 的 一 种 相机 ， 它 最 大 的 特点 是 可 以 通过 红外 
结构 光 或 Time-of-Flight (ToF) 原理 ， 像 激光 传 感 需 那样 ， 通 过 主动 回 

物体 妥 射 光 并 接收 返回 的 光 ， 测 出 物体 与 相机 之 间 的 距离 。 这 部 分 并 不 
像 双 目 相机 那样 通过 软件 计算 来 解决 ， 而 是 通过 物理 的 测量 手段 ， 所 以 
相 比 于 双 目 相机 可 节省 大 量 的 计算 ( 见 图 2-6〉。 目 前 党 用 的 RGB-D 相 

机 包括 Kinect/Kinect V2、Xtion Pro Live、RealSense 等 。 不 过 ， 现 在 多 
数 RGB-D 相 机 还 存在 测量 范围 罕 、 噪 声 大 、 视 野 小 、 易 有 日 光 王 扰 、 无 
法 测量 透射 材质 等 诸多 问题 ， 在 SLAM 方 面 ， 主 要 用 于 室内 ， 宇 外 则 较 
难 应 用 。 








图 2-6 RGB-D 数 据 : 深度 相机 可 以 下 接 测 量 物 体 的 图 像 和 距离 ， 从 而 恢复 三 维 结构 。 


我 们 讨论 了 几 种 常见 的 相机 ， 相 信 通 过 以 上 的 说 明 ， 你 已 经 对 它们 
有 J 了 直观 的 了 解 。 现 在 ， 想 象 相机 在 场景 中 运动 的 过 程 ， 我 们 将 得 到 一 
ANE LY EY 。 视 觉 SLAM 的 目标 ， 是 通过 这 样 的 一 些 图 像 ， 
进行 定位 和 地 图 构建 。 这 件 事情 并 没有 我 们 想象 的 那么 简单 。 它 不 是 某 
种 算法 ， 只 要 我 们 输入 数据 ， 束 可 以 往外 不 断 地 输出 定位 和 地 图 信息 
了 。SLAM 需 要 一 个 完善 的 算法 框架 ， 而 经 过 研究 者 们 长 期 的 努力 工 
作 ， 现 有 这 个 框架 已 经 定型 了 。 


2.2 ”经典 视觉 SLAM 框 架 


下 面 来 看 经 典 的 视觉 SLAM 框 架 ， 如 图 2-7 所 示 ， 了 解 一 下 视觉 
SLAM 究 葛 由 哪儿 个 模块 组 成 。 


前 端 后 请 
"— 
回环 检测 — 


图 2-7 ZARIE SLAME R] - 
整个 视 党 SLAM 流 程 包 括 以 下 步骤 。 
1. 传 感 器 信息 读 取 。 在 视觉 SLAM 中 主要 为 相机 图 像 信息 的 读 取 和 
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和 同步 。 


2. 人 视觉 里 程 计 (Visual Odometry, VO) 。 视 党 里 程 计 的 任务 是 估 
算 相 邻 图 像 间 相机 的 运动 ， 以 及 局 部 地 图 的 样子 。VO 又 称 为 前 端 
(Front End) 。 


3. 后 痪 优化  COptimization) o Ja imt sz AN TAY ZUR s EUER TT E 
的 相机 位 姿 ， 以 及 回环 检测 的 信息 ， 对 它们 进行 优化 ， 得 到 全 局 一 致 的 
轨迹 和 地 图 。 由 于 接 在 VO 之 后 ， 又 称 为 后 端 (Back End) 。 


4. 回 环 检测 (Loop Closing) 。 回 环 检测 判断 机 需 人 是 售 到 达 过 移 
前 的 位 置 。 如 果 检 测 到 回环 ， 写 会 把 信息 提供 给 后 端 进行 处 理 。 


5. 建 图 (Mapping) 。 它 根据 估计 的 轨迹 ， 建 立 与 任务 要 求 对 应 的 
地 图 。 
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序 库 中 提供 。 依 靠 这 些 算 法 ， 我 们 能 够 构建 一 个 视觉 SLAM 系 统 ， 使 之 
在 正常 的 工作 环 场 里 实时 定位 与 建 图 。 因 此 ， 我 们 说 ， 如 果 把 工作 环境 
限定 在 静态 、 刚 体 ， 光 照 变 化 不 明显 、 没 有 人 为 干扰 的 场景 ， 那 么 ， 
这 个 SLAM 系 统 是 相当 成 熟 的 了 外。 

证 者 可 能 还 没有 理解 上 面 几 个 模块 的 概念 ， 下 和 面 就 来 证 细 介绍 各 个 
模块 具体 的 任务 。 但 是 ， 准 确 理 解 其 工作 原理 需要 一 些 数 学 知识 ， 我 们 
将 放 到 本 书 的 第 二 部 分 进行 。 这 里 读者 只 需 对 各 模块 有 一 个 直观 的 、 定 
性 的 理解 即 可 。 


2.2.1 fiw HET 


视 沉 里程 计 关心 的 是 相 邻 图 像 ”之 间 的 相机 运动 ， 了 最 简单 的 情况 当 
处 是 两 张 图 像 乙 间 的 运动 天 系 。 例 如 ， 当 看 到 图 2-8 时 ， 我 们 会 目 然 地 
反应 出 右 赂 应 该 是 下 图 同 堪 旋转 一 定 角度 的 结束 《在 视频 情况 下 感觉 会 
更 加 目 然 ) 。 我 们 不 妨 思 考 一 下 : 目 己 是 怎么 知道 <* 问 元 旋转 ”这 件 事 情 
FUE? 人 奖 早 已 习惯 于 用 眼睛 探索 世界 ， 佑 计 目 己 的 位 置 ， 但 义 往往 难 
以 用 理性 的 语言 揪 述 我 们 的 下 党 。 看 到 图 2-8 时 ， 我 们 会 目 然 地 认为 ， 
这 个 场景 中 离 我 们 近 的 是 吧台 ， 远 处 古 墙壁 和 黑板 。 当 相机 同 左 转动 
时 ， 吧 人 台 离 我 们 近 的 部 分 出 现在 视野 中 ， 而 右 侧 远 处 的 柜子 则 移出 了 视 
野 。 通 过 这 些 信 息 ， 我 们 判断 相机 应 该 是 问 左 旋转 了 了 。 


ZN 
人 眼 反应 的 运动 方向 





图 2-8 ”相机 提 摄 到 的 图 片 与 人 眼 反 应 的 运动 方 癌 。 


但 是 ， 如 条 进一步 问 : BET WARE eRe S EDR, FE BD HOR? 
我 们 束 很 难 给 出 一 个 确切 的 答案 了 。 因 为 我 们 的 直觉 对 这 些 具 体 的 数字 
并 不 敏感 。 但 是 ， 在 计算 机 中 ， 驻 必须 精确 地 测量 这 段 运动 信息 。 所 以 
我 们 要 问 : 计算 机 是 如 何 通 过 图 像 确 定 相 机 的 运动 的 呢 ? 


前 面 也 提 过 ， 在 计算 机 视觉 领域， 人 类 在 和 直 党 上 看 来 十 分 目 然 的 事 
情 ， 在 计算 机 视 党 中 却 非常 困难 。 图 像 在 计算 机 里 只 是 一 个 数值 矩阵 。 
这 个 矩阵 里 表达 着 什么 东西 ， 计 算 机 旦 无 概念 〈 这 也 正 是 现在 机 需 学 习 
要 解决 的 问题 )》。 而 在 视觉 SLAM 中 ， 我 们 只 能 看 到 一 个 个 像素 ， 知 道 
它们 是 某 些 空间 点 在 相机 的 成 像 平 面 上 投影 的 结果 。 所 以 ， 为 了 定量 地 
估计 相机 运动 ， 必 须 先 了 解 相 机 与 空间 点 的 几何 关系 。 


要 讲 清 这 个 几何 关系 以 及 VO 的 实现 方法 ， 需 要 铺垫 一 些 背 景 知 
识 。 在 这 里 我 们 先 让 读者 对 VO 有 个 直观 的 概念 。 现 在 只 需 知 道 ，VO 能 
够 通过 相 邻 帆 间 的 图 像 估 计 相 机 运动 ， 并 恢复 场景 的 空间 结构 。 称 它 
为 “里 程 计 ”是 因为 它 和 实际 的 里 程 计 一 样 ， 只 计算 相 邻 时 刻 的 运动 ， 而 
和 再 往 前 的 过 去 的 信息 没有 关联 。 在 这 一 点 上 ，VO 就 像 一 种 只 有 短 时 
间 记 忆 的 物种 。 


现在 ， 假 定 我 们 已 有 了 一 个 视觉 里 程 计 ， 估 计 了 两 张 图 像 间 的 相机 
运动 。 那 么 ， 只 要 把 相 邻 时 刻 的 运动 “ 串 ” 起 来 ， 束 构成 了 机 右 人 的 运动 
轨迹 ， 从 而 解决 了 定位 问题 。 另 一 方面 ， 我 们 根据 每 个 时 刻 的 相机 位 
置 ， 计 算出 各 像素 对 应 的 空间 点 的 位 置 ， 就 得 到 了 地 图 。 这 么 说 来 ， 有 
f VO, ANWR f SLAMIR BUE ? 

视 党 里 程 计 确实 是 SLAM 的 关键 ， 我 们 也 会 花 大 量 的 篇 幅 来 介绍 
它 。 然 而 ， 仅 通过 视觉 里 程 计 来 估计 轨迹 ， 将 不 可 避免 地 出 现 累 积 漂移 
(Accumulating Drift) 。 这 古 由 于 视 党 里 程 计 《在 最 简单 的 情况 下 ) 只 
估计 两 个 图 像 间 的 运动 造成 的 。 我 们 知道 ， 每 次 估计 都 带 有 一 定 的 误 
莽 ， 而 由 于 里 程 计 的 工作 方式 ， 先 前 时 刻 的 误 兰 将 会 传递 到 下 一 时 刻 ， 
导致 经 过 一 段 时 间 之 后 ， 估 计 的 轨迹 将 不 再 准确 〈 见 图 2-9) 。 比 方 
说 ， 机 器 人 先 同 左 转 90”， 再 同 右 转 90"”。 由 于 误差 ， 我 们 把 第 一 个 90 ^ 
估计 成 了 89 ”。 那 我 们 就 会 尴 界 地 发 现 ， 疝 右 转 之 后 机 器 人 的 估计 位 置 
并 没有 回 到 原点 。 更 糟糕 的 是 ， 即 使 之 后 的 估计 再 准确 ， 与 真实 值 相 
比 ， 都 会 带 上 这 -1 的 误差 。 





累积 误差 导致 
长 时 间 估 计 不 再 准确 | 
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图 2-9 RRRA rd REA AR 。 


这 也 就 是 所 谓 的 漂移 (Drift) 。 它 将 导致 我 们 无 法 建立 一 致 的 地 
图 。 你 会 有 友 现 原本 下 的 走廊 变 成 了 冬 的 ， 而 原本 90 ^ EN ELA eR S ER 
一 一 这 实在 是 一 件 很 难 令 人 妨 受 的 事情 ! 为 了 解决 深 移 问题 ， 我 们 还 需 
要 两 种 技术 : 后 端 优化 是 和 回环 检测 。 回 环 检测 负责 把 “机 大 人 回 到 原 
ua 
形状 。 


2.2.2 ”后 端 优化 


笼统 地 说 ， 后 端 优化 主要 指 处 理 SLAM 过 程 中 噪声 的 问题 。 虽 然 我 
们 很 希望 所 有 的 数据 都 是 准确 的 ， 然 而 现实 中 ， 有 再 精确 的 传感器 也 带 有 
一 定 的 噪声 。 便 宜 的 传 感 右 测量 误 兰 较 大 ， 导 贯 的 可 能 会 小 一 些 ， 有 的 
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相机 运动 ”之 外 ， 我 们 还 要 关心 这 个 估计 融 有 多 大 的 噪声 ， 这 些 噪声 是 
如 何 从 上 一 时 刻 传 递 到 下 一 时 刻 的 ， 而 我 们 又 对 当前 的 估计 有 多 大 的 目 
信 。 后 山 优化 要 考虑 的 问题 ， 了 束 是 如 何 从 这 些 珊 有 了 噪声 的 数据 中 估计 整 
个 系统 的 状态 ， 以 及 这 个 状态 估计 的 不 确定 性 有 多 大 一 一 这 称 为 最 大 后 
验 概 率 估 计 CMaximum-a-Posteriori; MAP) 。 这 里 的 状态 既 包 括 机 器 
人 目 映 的 轨迹 ， 也 包 人 名 地 图 。 


相对 地 ， 和 视觉 里 程 计 部 分 有 时 和 被 称 为 "前 病 ”。 在 SLAM 械 架 中 ， 前 





站 给 后 提 供 待 优化 的 数据 ， 以 及 这 些 数 据 的 初始 值 。 而 后 端 负责 整体 
的 优化 过 程 ， 它 往往 面 对 的 只 有 数据 ， 不 必 关 心 这 些 数 据 到 压 来 日 什么 
传感器 。 在 视觉 SLAM 中 ， 前 端 和 计算 机 视觉 研究 领域 更 为 相关 ， 比 
如 图 像 的 特征 提取 与 匹配 等 ， 后 痪 则 主要 是 滤波 与 非 线 性 优化 算法 。 

从 历史 意义 上 来 说 ， 现 在 我 们 称 为 后 姗 优化 的 部 分 ， 很 长 一 段 时 间 
直接 被 称 为 "SLAM 研 究 ”。 早 期 的 SLAM 问 题 是 一 个 状态 估计 问题 一 一 
下 是 后 端 优化 要 解决 的 东西 。 在 了 最早 提出 SLAM 的 一 系列 论文 中 ， 妆 时 
的 人 们 称 它 为 “空间 状态 不 确定 性 的 估计 ”(Spatial Uncertainty) H 。 
虽然 有 一些 星 深 ， 但 也 确实 反映 出 了 SLAM 问 题 的 本 质 : 对 运动 主体 目 
号 和 周围 环境 空间 不 确定 性 的 估计 。 为 了 解决 SLAM 问 题 ， 我 们 需要 
状态 估计 理论 ， 把 定位 和 建 图 的 不 确定 性 表达 出 来 ， 然 后 采用 滤波 兹 或 
非 线 性 优化 ， 舍 计 状 态 的 均值 和 不 确定 性 《方面 ) 。 状 态 估 计 与 非 线性 
优化 的 具体 内 容 将 在 第 6 讲 、 第 10 讲 和 第 11 讲 介绍 。 让 我 们 暂时 跳 过 它 
的 原理 说 明 ， 继 续 往 下 介绍 。 


2.2.3 ”回环 检测 


回环 检测 ， 又 称 闭 环 检测 (Loop Closure Detection) ， 主 要 解决 位 
置 估计 随时 间 尝 移 ” 的 问题 。 怎 么 解决 呢 ? 假设 实际 情况 下 机 喜人 经 过 
一 段 时 间 的 运动 后 回 到 了 原点 ， 但 是 由 于 漂移 ， 它 的 位 置 估计 值 却 没 有 
回 到 原点 。 怎 么 办 呢 ? RIE, WRAP LEHLA REE E 
了 原点 ”这 件 事 ， 或 者 把 “原点 ”识别 出 来 ， 我 们 再 把 位 置 估 计 值 “ 拉 ” 过 
去 ， 束 可 以 消除 漂移 了 。 这 束 是 所 谓 的 回环 检测 。 

回环 检测 与 “定位 ”和 “ 建 图 ”二 者 都 有 和 密切 的 关系 。 事 实 上 ， 我 们 认 
为 ， 地 图 存在 的 主要 意义 是 让 机 需 人 知晓 目 己 到 过 的 地 方 。 为 了 实现 回 
环 检测 ， 我 们 需要 让 机 磊 人 具有 识别 到 过 的 场景 ”的 能 力 。 它 的 实现 手 
段 有 很 多 。 例 如 像 前 面 说 的 那样 ， 我 们 可 以 在 机 部 人 下 方 设 置 一 个 标志 
物 〈 如 一 张 二 维 码 图 厂 )。 它 只 要 看 到 了 这 个 标志 ， 束 知道 目 己 回 到 了 
原点 。 但 是 ， 该 标志 物 实 质 上 是 一 种 环境 中 的 传 感 希 ， 对 应 用 环境 做 了 
限制 (万 一 不 能 贴 二 维 码 怎么 办 ? ) 。 我 们 更 希望 机 器 人 能 使 用 携带 的 
传感器 一 一 也 就 是 图 保 本 号， 来 完成 这 一 任务 。 例 如 ， 可 以 判断 图 像 间 
的 相似 性 ”来 完成 回环 检测 。 这 一 点 和 人 是 相似 的 。 当 我 们 看 到 两 张 相 
似 的 图 片 时 ， 容 易 辨 认 它 们 来 目 同 一 个 地 方 。 如 末 回 环 检测 成 功 ， 可 以 


显著 地 减 小 素 积 误 苦 。 所 以 ， 视 觉 回环 检 训 实质 上 是 一 种 计算 图 像 数 扼 
相似 性 的 算法 。 由 于 图 像 的 信息 非常 丰 吾 ， 使 得 正确 检 训 回环 的 难度 降 
低 了 不 少 。 

在 检测 到 回环 之 后 ， 我 们 会 把 “A 与 B 是 同一 个 点 ”这 样 的 信息 告诉 
后 哨 优 化 算法 。 然 后 ， 后 姗 根据 这 些 新 的 信息 ， 把 轨迹 和 地 图 调整 到 符 
合 回 环 检测 结果 的 样子 。 这 样 ， 如 来 我 们 有 充分 而 且 正 确 的 回环 检 柚 ， 
束 可 以 消除 素 积 误 寺 ， 得 到 全 局 一 致 的 轨迹 和 地 图 。 


2.2.4 ÆR 


ÆR] (Mapping) 是 指 构建 地 图 的 过 程 。 地 图 〈 见 图 2-10) 是 对 环 
境 的 摘 述 ， 但 这 个 摘 述 并 不 是 固定 的 ， 需 要 视 SLAM 的 应 用 而 定 。 





2D 栅 格 地 图 2D 拓扑 地 图 





3D 点 云 地 图 3D 网 格 地 图 


图 2-10 ”形形色色 的 地 图 号 。 


对 于 家 用 扫地 机 融 人 来 访 ， 这 种 主要 在 低 矮 平面 里 运动 的 机 硕 人 ， 
只 需要 一 个 二 维 的 地 图 ， 标记 哪里 可 以 通过 哪里 存在 障 但 物 ， 束 够 它 


在 一 定 范 围 内 导航 了 了。 而 对 于 一 个 相机 ， 和 有 6 目 由 度 的 运动 ， 我 们 至 
少 再 要 一 张 三 维 的 地 图 。 有 些 时 候 ， 我 们 想 归 一 个 漂 胸 的 重建 结 末 ， 不 
仅 是 一 组 空间 点 ， 还 需要 带 纹 理 的 三 角 面 片 。 另 一 些 时 候 ， 我 们 又 不 天 
心地 图 的 样子 ， 只 需要 知道 “A 点 到 B 点 可 通过 ， 而 B 点 到 C 点 不 行 > 这 样 
的 事情 。 长 全， 有 时 不 珊 要 地 图 ， 或 者 地 图 可 以 由 其 他 人 捉 供 ， 例 如 ， 
行驶 的 车 辆 往往 可 以 得 到 已 绘制 好 的 当地 地 图 。 


对 于 地 图 ， 我 们 有 太 多 的 想法 和 需求 。 因 此， 相 比 于 前 面 近 到 的 视 
觉 里 程 计 、 回 环 检 调 和 后 姗 优化 ， 建 图 并 没有 一 个 固定 的 形式 和 算法 。 
一 组 空间 点 的 集合 也 可 以 称 为 地 图 ， 一 个 永 涡 的 3D 模 型 尔 是 地 图 ， 一 
个 标记 看 城市 、 村 庄 、 铁 路 、 河 道 的 图 片 还 是 地 图 。 地 图 的 形 却 随 
SLAM 的 应 用 场合 而 定 。 大 体 上 讲 ， 可 以 分 为 度量 地 图 与 拓扑 地 图 两 
种 。 


FE atte AC Metric Map) 


FE Fe | o8 USD Ze d A] A OS RE H st 
(Sparse) 5/5 (Dense) 对 其 分 类 。 稀 玖 地 图 进行 了 一 定 程度 的 抽 
象 ， 并 不 需要 表达 所 有 的 物体 。 例 如 ， 我 们 选择 一 部 分 共有 代表 意义 的 
东西 ， 称 之 为 路 标 〈Landmark) ， 那 么 一 张 黎 琥 地 图 残 是 由 路 标 组 成 的 
HHA, MARRIED Ay CA ma et. RH, eae Ete Re Be 
AYA BUA ZR PG. ON TERR UL, Pee pt ES 了。 而 用 于 导航 
IY, WU GEE rs Zee ae A) CS ee EPA eR ZARA Ip? ) 。 
Aj n Ha PRISED as FRR ES, LT PV) RA. ERE Be Hh AS 
是 许多 个 小 格子 (Grid) ， 而 对 于 三 维度 量 地 图 则 是 许多 小 方块 
(Voxel) 。 一 般 地 ， 一 个 小 块 含有 占据 、 空 几 、 未 知 三 种 状态 ， 以 表 
达 访 格 内 是 个 有 物体 。 当 奏 询 有 个 空间 位 置 时 ， 地 图 能 够 给 出 该 位 置 是 
含 可 以 通过 的 信息 。 这 样 的 地 图 可 以 用 于 各 种 导航 算法 ， 如 A*、D*le 
等 ， 为 机 硕 人 研究 者 所 重视 。 但 是 我 们 也 看 到 ， 这 种 地 图 需要 存储 每 一 
个 格 点 的 状态 ， 会 耗 绩 大 量 的 存储 空间 ， 而 且 多 数 情 况 下 地 图 的 许多 细 
节 部 分 是 无 用 的 。 男 一 方面 ， 大 规模 度量 地 图 有 时 会 出 现 一 致 性 问题 。 
很 小 的 一 点 转 问 谋 锚 ， 可 能 会 叶 致 两 间 屋 子 的 增 出 现 午 达 ， 使 地 图 失 
A o 

拓扑 地 图 〈 Topological Map? 

相 比 于 度量 地 图 的 精确 性 ， 拓 扑 地 图 则 更 强调 地 图 元 系 之 间 的 关 


系 。 拓 扑 地 图 是 一 个 图 (Graph，〉， 由 节点 和 边 组 成 ， 只 考虑 节 扣 间 的 
连通 性 ， 例 如 A、B 扣 是 连通 的 ， 而 个 和 考虑 如 何 从 A 扩 到 达 B 操 。 它 放松 
了 地 图 对 精确 位 置 的 需要 ， 去 择 了 地 图 的 细节 问题 ， 是 一 种 更 为 紧 读 的 
表达 方式 。 然 而 ， 拓 扑 地 图 不 擅长 表达 具有 复 傈 结构 的 地 图 。 如 何 对 地 
图 进行 分 割 形成 结 扣 与 边 ， 义 如 何 使 用 拓扑 地 图 进行 导航 与 路 径 规 划 ， 
Vie A FF WE FC HY |] pel 


2.3 SLAM 问 题 的 数学 表述 


通过 前 面 的 介绍 ， 读 者 应 该 对 SLAM 中 各 个 模块 的 组 成 和 主要 功能 
有 了 直观 的 了 解 。 但 仅仅 靠 直观 印象 并 不 能 帮助 我 们 写 出 可 以 运行 的 程 
序 。 我 们 要 把 它 上 升 到 理性 层次 ， 也 就 是 用 数学 语言 来 描述 SLAM 过 
程 。 我 们 会 用 到 一 些 变量 和 公式 ， 但 请 读者 放心 ， 我 们 会 尽量 让 它 保 持 
足够 地 清楚 。 

假设 小 葛 卜 正 携带 着 某 种 传感器 在 未 知 环境 里 运动 ， 怎 么 用 数学 语 
言 描述 这 件 事 呢 ? 首先 ， 由 于 相机 通常 是 在 某 些 时 刻 采 集 数 据 的 ， 所 以 
我 们 也 只 关心 这 些 时 刻 的 位 置 和 地 图 。 这 就 把 一 段 连 续 时 间 的 运动 变 成 
了 离散 时 刻 t =1,…,K 当中 发 生 的 事情 。 在 这 些 时 刻 ， 用 x Zep! bh 
身 的 位 置 。 于 是 各 时 刻 的 位 置 就 记 为 x x BIENES NI 
迹 。 地 几 方 面 ， 我 们 假设 地 图 是 由 许多 个 路 标 ” ‘Landmark) 组 成 的 ， 
而 每 个 时 刻 ， 传 感 器 会 测量 到 一 部 分 路 标点 ， 得 到 它们 的 观测 数据 。 不 
妨 设 路 标点 一 共有 N 个 ， 用 y yy 表示 它们 。 在 这 样 的 设 定 中 , "INE 
携带 着 传感器 在 环境 中 运动 ?由 如 下 两 件 事情 描述 : 

1. 什 么 是 运动 ?我 们 要 考虑 从 k- 1 时 刻 到 k 时 刻 ， 小 更 卜 的 位 置 x 是 
如 何 变 化 的 。 

2. 什 么 是 观测 ? 假设 小 葛 下 在 K 时 刻 于 x， 处 探测 到 了 某 一 个 路 标 》 
， 我 们 要 考虑 这 件 事情 是 如 何 用 数学 语言 来 描述 的 。 

先 来 看 运动 。 通 彰 ， 机 器 人 会 携带 一 个 测量 上 自身 运动 的 传 感 项 ， 比 
如 说 码 盘 或 惯性 传感器 。 这 个 传感器 可 以 测量 有 关 运 动 的 读数 ， 但 不 一 
定 直 接 就 是 位 置 之 差 ， 还 可 能 是 加 速度 、 和 角速度 等 信息 。 然 而 ， 无 论 是 
什么 传感器 ， 我 们 都 能 使 用 一 个 通用 的 、 抽 象 的 数学 模型 : 


zc = f (k-ti, Uk, Wk) - (2.1) 


XE, u, EISSN CAIN TEA A ) ，w 为 噪声 。 注 
意 到 ， 我 们 用 一 个 一 般 函 数 f 来 插 述 这 个 过 程 ， 而 不 上 其 体 指 明 f 的 作用 方 
式 。 这 使 得 整个 隙 数 可 以 指 代 任 童 的 运动 传 感 费 ， 成 为 一 个 退 用 的 方 


程 ， 而 不 必 限 定 于 某 个 特殊 的 传感器 上 。 我 们 把 它 称 为 运动 方程 。 

与 运动 方程 相对 应 ， 还 有 一 个 观测 方程 ”。 观 测 方程 描述 的 是 ， 当 
小 葛 下 在 x 位 置 上 看 到 某 个 路 标点 y, ， 产 生 了 一 个 观测 数据 z,，。 同 样 ， 
用 一 个 抽象 的 函数 h 来 描述 这 个 关系 : 


二， (2.2) 


XE, Vy Fe XX UOI] E. BER xS V H FIM Pr H ES 7 J a8 E 
多 ， 这 里 的 观测 数据 z 以 及 观测 方程 h 也 有 许多 不 同 的 形式 。 

读者 或 许 会 说 ， 我 们 用 的 函数 fh ”， 似 乎 并 没有 上 有 具体 地 说 明 运 动 和 
观测 是 怎么 回 事 ? 同时 ， 这 里 的 x ,y ,z 又 是 什么 东西 呢 ? 事实 上 ， 根 据 
小 东 卜 的 真实 运动 和 传 感 右 的 种 类 ， 存 在 厦大 干 种 参数 化 293 
(Parameterization) 。 什 么 叫 参 数 化 呢 ? zs va. f E PIER IBI 
Hie, MBA, EHDA HPPA RGA, Bix, = 
awe. — ED. EBA eS He ell ee B/N Ob EE PT Td DE. 
和 转角 的 变化 量 w = [Ac Ay, A, 于 是 ， 此 时 运动 方程 束 可 以 具体 化 为 
这 是 集 单 的 线性 关系 。 不 过 ， 并 不 是 所 有 的 传 感 句 都 能 直接 测量 出 位 移 
和 角 虚 变化 ， 所 以 也 存在 痢 其 他 形式 更 加 复杂 的 运动 方程 ， 那 时 我 们 可 
能 需要 进行 动力 学 分 析 。 关 于 观测 方程 ， 比 方 说 小 更 卜 携 市 看 一 个 二 维 
油光 传感器 。 我 们 知道 油光 传 感 右 观测 一 个 2D 路 标点 时 ， 能 够 测 到 两 
ME: 路 标点 与 小 萝卜 本 体 之 间 的 距离 r 和 夹 角 pg 。 记 路 标点 为 y =[p, ,p， 


| 《为 你 持 简 洁 ， 和 省 略 了 下 标 》， 观 测 数 据 为 z né 了 ， 那 么 观测 方程 
SUAM: 


T T Ax 
=| 4 十 | Ay + Wr. (2.3) 
A0 
k k—1 k 





2 2 
r E (Px c. £) EA (Py v y) rv (2.4) 
Ó arctan (B=) 


rcc 


考虑 帘 沈 SLAM 时 ， 传 感 占 是 相机 ， 那 么 观测 方程 就 是 “对 路 标 扣 
扣 报 后， 得 到 图 像 中 的 像 系 * 的 过 程 。 这 个 过 程 军 涉 到 相机 模型 的 插 
述 ， 将 在 第 5 讲 中 详细 介绍 ， 这 里 暂且 略 过 。 

可 见 ， 针 对 不 同 的 传 感 融 ， 这 两 个 方程 有 个 同 的 参数 化 形式 。 如 来 
我 们 你 持 通 用 性 ， 把 它们 取 成 通用 的 抽象 形式 ， 那 么 SLAM 过 程 可 忌 结 
为 两 个 基本 方程 : 


(2.5) 
Ak. 一 h (Yj, Tk, vk s) 


| Ge = f (Xp) 


这 两 个 方程 描述 了 最 基本 的 SLAM 问 题 : 当知 道 运动 测量 的 读数 4 
， 以 及 传 感 占 的 读数 z” 时， 如 何 求解 定位 问题 “估计 x ”) 和 建 图 问题 
(估计 y ) ? 这 时 ， 我 们 就 把 SLAM 问 题 建 模 成 了 一 个 状态 估计 问题 : 
如 何 通 过 市 有 了 虹 声 的 测量 数据 ， 估 计 内 部 的 、 隐 藏 看 的 状态 变量 ? 


状态 估计 问题 的 求解 ， 与 两 个 方程 的 具体 形式 ， 以 及 噪声 服从 哪 种 
分 布 有 关 。 按 照 运 动 和 观测 方程 是 否 为 线性 ， 品 声 是 否 服 从 融 斯 分 布 进 
行 分 类 ， 分 为 线性 / 非 线 性 和 高 期 JEAN 系统 。 其 中 线性 融 斯 系统 
(Linear Gaussian，LG 系 统 ) 是 最 简单 的 ， 它 的 无 侦 的 最 优 估 计 可 以 由 
卡尔 曼 滤 波 器 (Kalman Filter, KF) 给 出 。 而 在 复杂 的 非 线性 非 高 期 系 
Zt (Non-Linear Non-Gaussian，NLNG 系 统 ) 中 ， 我 们 会 使 用 以 扩展 卡 
尔 受 滤波 器 CExtended Kalman Filter, EKF) 和 非 线 性 优化 两 大 类 方法 
去 求解 。 直 至 21 世 纪 早 期 ， 以 EKF 为 主 的 滤波 右 方 法 在 SLAM 中 占据 了 
主导 地 位 。 我 们 会 在 工作 点 处 把 系统 线性 化 ， 并 以 预测 一 更 新 两 大 步 又 
进行 求解 〈 见 第 10 讲 ) 。 最 早 的 实时 视 沉 SLAM 系统 即 是 基于 EKFW JT 
发 的 。 随 后 ， 为 了 克服 EKE 的 缺点 〈 例 如 线性 化 误 甜 和 噪声 高 斯 分 布 假 
设 ) ， 人 们 开始 使 用 粒子 滤波 器 (Particle Filter) 等 其 他 滤波 器 ， 旋 至 
使 用 非 线 性 优化 的 方法 。 时 至 今日 ， 主 流 视 觉 SLAM 使 用 以 图 优化 
(Graph Optimization) 为 代表 的 优化 拉 术 进行 状态 估计 5。 我 们 认为 
DAG GZ ELT VE A BOR, RT, a A i A F 
使 用 优化 方法 〈 见 第 10 讲 和 第 11 讲 ) 。 


相信 读者 已 经 对 SLAM 的 数学 模型 有 了 大 致 的 了 解 ， 然 而 我 们 仍 需 
澄清 一 些 问题 。 首 先 ， 要 说 明 机 器 人 位 置 x 是 什么 。 我 们 方才 说 位 置 上 


有 些 模糊 的 。 也 许 读者 能 够 理解 ， 在 平面 中 运动 的 小 蓝 上 下 可 以 用 两 个 坐 
标 加 一 个 转角 的 形式 将 位 置 参数 化 。 然 而 ， 虽 然 我 的 漫画 风格 有 些 二 次 
元 ， 小 画 卜 在 更 多 时 候 古 一 个 三 维 空 间 里 的 机 器 人 。 我 们 知 志 三维 空间 
的 运动 由 3 个 轴 构 成 ， 所 以 小 画 卜 的 运动 要 由 3 个 轴 上 的 平移 ， 以 及 旨 
3 个 轴 的 旋转 来 描述 ， 一 共有 6 个 目 由 度 。 那 是 个 意味 着 随便 用 一 个 Rs 
中 的 癌 量 惑 能 摘 述 它 了 呢 ? 我 们 将 肥 现 事情 并 没有 那么 简单 。 对 6 目 由 
及 的 位 姿 9, ， 如 何 表达 它 ， 如 何 优化 它 ， 痢 需要 一 定 饥 幅 来 介绍 ， 这 将 
征 第 3 讲 和 第 4 讲 的 主要 和 内容。 随后 ， 我 们 要 说 明 在 视觉 SLAM 中 ， 观 讽 
方程 ”如 何 参数 化 。 换 名 话说 ， 衬 间 中 的 路 标点 是 如 何 投影 到 一 张 照 片 
上 有 的 。 这 需要 解释 相机 的 成 像 模 型 ， 我 们 将 在 第 5 讲 介绍 。 最 后 ， 当 知 
道 了 这 些 信息 ， 怎 么 求解 上 述 方 程 ? 这 需要 非 线性 优化 的 知识 ， 是 第 6 
讲 的 内 容 。 

这 些 内 容 组 成 了 本 书 数学 知识 的 部 分 。 在 对 它们 进行 铺垫 之 后 ， 我 
们 残 能 仔细 讨论 视觉 里 程 计 、 后 闯 优 化 等 更 详细 的 知识 了 。 可 以 看 到 ， 
本 讲 介 绍 的 内 容 构 成 了 本 书 的 一 个 提要 。 如 果 读 者 还 没有 很 好 地 理解 上 
面 的 概念 ， 不 妨 回 过 头 再 阅读 一 届 。 下 面 融 要 开始 介绍 程序 啦 ! 


2.4 ”实践 编程 基础 


2.4.1 ”安装 Linux 操 作 系 统 


终于 开始 令 人 兴奋 的 实践 环节 啦 ! 你 是 否 准备 好 了 呢 ? 为 了 完成 本 
书 的 实践 环节 ， 我 们 需要 准备 一 台电 脑 。 你 可 以 使 用 笔记 本 或 台式 机 ， 
当然 最 好 是 你 个 人 的 电脑 ， 因 为 我 们 需要 在 上 面 安装 操作 系统 进行 实 
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我 们 的 程序 将 以 Linux 上 的 C++ 程序 为 主 。 在 实验 过 程 中 ， 我 们 会 使 
用 到 大 量程 序 库 。 大 部 分 程序 库 只 对 Linux 提 供 了 较 好 的 支持 ， 而 在 
Windows 上 的 配置 则 相对 〈 相 当 ) 厂 烦 。 因 此 ， 我 们 不 得 不 假定 你 已 经 
具备 天 于 Linux 的 其 本 知识 了 (参见 上 一 讲 的 练习 题 ) ， 包 括 使 用 基本 
的 命令 ， 了 解 软件 如 何 安装 。 这 样 我 们 才 无 顷 讲 解 这 些 内 容 。 当 然 ， 你 
不 必 了 解 如 何在 Linux 下 开发 C++ 程序 ， 这 正 是 下 面 会 详细 讲解 的 。 


我 们 先 来 搭建 本 书 所 需 的 实验 环境 。 作 为 一 本 面 癌 初学 者 的 书 ， 我 
们 使 用 Ubuntu 作为 开发 环境 。 在 Linux 的 各 大 发 行 版 中 ，Ubuntu 及 其 衍 
生 版 本 一 直 享 有 对 用 户 友好 的 美誉 。Ubuntu 是 一 个 开源 操作 系统 ， 它 的 
系统 和 软件 可 以 在 官方 网 站 Chttp://cn.ubuntu.com) 免费 下 载 ， 并 且 提 
供 了 详细 的 安装 方式 说 明 。 同 时 ， 清 华 、 中 科大 等 国内 各 大 高 校 也 提供 
了 Ubuntu 软 件 产 ， 使 软件 的 安装 十 分 便捷 。 对 于 初学 者 ， 建 议 你 使 用 和 
我 们 一 样 的 环境 : Ubuntu 14.04。 如 果 你 想 试 试 其 他 口味 ， 那 么 Ubuntu 
16.04、Ubuntu Kylin、Debian 7/8 和 Linux Mint 17/18 也 是 不 错 的 选择 。 
我 们 保证 书 中 所 有 代码 在 Ubuntu 14.04 下 经 过 了 良好 的 测试 ， 但 如 果 你 
选择 其 他 发 行 版 ， 则 无 法 确定 你 是 否 会 过 到 问题 。 你 可 能 需要 花费 一 些 
时 间 解 决 问题 〈 不 过 你 也 可 以 把 它们 当 作 锻炼 目 己 的 机 会 ) 。 大 体 来 
说 ，Ubuntu 对 各 种 库 的 文 持 均 较为 完善 ， 软 件 也 非常 丰 画 。 尽 管 我 们 不 
限制 你 具体 使 用 哪 种 Linux 发 行 版 ， 不 过 在 讲解 中 ， 我 们 会 以 ^ Ubuntu 
14.04 为 例 ， 且 主要 使 用 Ubuntu 下 的 命令 〈 例 如 apt-get) ， 束 不 谈 在 其 
他 Linux 下 怎么 操作 了。 一 上 般 情况 下 ， 程 序 在 Linux 则 移植 不 会 非常 烦 
琐 。 但 如 果 你 想 在 Windows 或 OS X 下 使 用 本 书 中 的 程序 ， 则 需要 有 一 定 
的 移植 经 验 。 


现在 ， 请 大 家 在 目 己 的 PC 上 和 安装 好 Ubuntu 14.04。 关 于 Ubuntu 的 安 
汪 ， 可 以 在 网 上 搜 到 大 量 教 程 ， 只 要 照 做 即 可 ， 此 处 略 过 。 最 简单 的 方 
式 是 使 用 虚拟 机 【〈 见 图 2-11) ， 但 需要 占用 大 量 内 存 〈 我 们 的 经 验 是 
4GB 以 上 ) 和 CPU 才能 保持 流畅 :你 也 可 以 安装 双 系 统 ， 这 样 会 快 一 
些 ， 但 需要 一 个 空 日 的 U 稚 来 作为 局 动 禹 。 男 外 ， 虚 拟 机 软件 对 外 部 便 
件 的 文 持 往 往 不 够 好 ， 如 果 和 硕 望 使 用 实际 的 传 感 右 〈 双 目 、Kinect 
等 ) ， 则 建议 你 使 用 双 系 统 来 安装 Linux。 


#@ xp Gh) BEM KUM) GNO WE dH Bii 'e H V | [E y Ubuntu 1404. ^ 
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图 2-11 一 个 运行 在 虚拟 机 中 的 Ubuntu 14.04. 
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果 你 有 SSD 便 盘 ， 这 个 过 程 大 概 用 时 15 分 钟 。 

“安装 完成 后 ， 请 务必 把 软件 源 设 置 到 离 你 较 近 的 服务 占 上 ， 以 获 
得 更 快 的 下 载 速度 。 例 如 我 们 使 用 清华 的 软件 产 通 稼 能 以 10MB/s 的 速 
FE ELAR ER TPE! 。 

现在 ， 假 设 你 已 经 成 功 安 装 好 Ubuntu， 无 论 是 使 用 虚拟 机 还 是 双 系 
统 的 方式 。 如 果 你 还 不 误 悉 Ubuntu， 可 以 试 试 它 的 各 种 软件 ， 体 验 一 下 
它 的 界面 和 交互 方 式 00 。 不 过 我 必须 提醒 你 ， 特 别 是 新 手 有 朋友: 不 要 
在 Ubuntu 的 用 户 界 面 上 花费 太 多 时 间 ! Linux 有 许多 可 能 浪费 时 间 的 地 


方 ， 你 可 能 会 找到 某 些小 众 的 软件 、 一 些 游戏 ， 甚 至 会 为 找 一 张 壁 纸 花 
费 不 少时 | 则 。 但 是 请 记 住 ， 你 是 用 Linux 来 工作 有 的。 特别 是 在 本 书 中 ， 
你 是 用 Linux 来 学 习 SLAM 的 ， 所 以 要 尽量 把 时 间 花 在 学 习 SLAM E. 


好 了 ， 我 们 选择 一 个 目录 ， 放 置 本 书 中 SLAM 程 序 的 代码 。 例 如 ， 
可 以 将 代码 放 到 家 目 孙 Chome) 的 “slambook” 下 。 以 后 我 们 将 把 这 个 
目录 称 为 “代码 根 目 录 。、”。 同 时 ， 可 以 男 外 选择 一 个 目录 ， 把 本 书 的 Git 
代 公 复制 下 来 ， 方 便 做 实验 时 随时 对 照 。 本 书 的 代 人 码 是 按 章 节 划 分 的 。 
比如 ， 本 讲 的 代码 将 在 slambook/ch2 下 ， 下 一 讲 则 在 slambook/ch3 下 。 
所 以 ， 现 在 请 读者 进入 slambook/ch2 下 (你 应 该 会 狐 建 文件 夹 并 进入 访 
VIFK IE) 


2.4.2 Hello SLAM 


我 们 从 最 基本 的 程序 开始 。 与 许多 计算 机 类 书籍 一 样 ， 我 们 来 书写 
一 个 HelloSLAM 程 序 。 不 过 在 做 这 件 事 之 前 ， 我 们 先 来 聊 聊 程序 是 什 
Zo 


在 Linux 中 ， 程 序 是 一 个 具有 执行 权限 的 文件 。 它 可 以 是 一 个 脚 
本 ， 了 也 可 以 是 一 个 二 进 制 文件 ， 不 过 我 们 不 限定 它 的 后 级 名 不 像 
Windows 那 样 需 要 指定 成 .exe 文 件 ) 。 我 们 第 用 的 cd、ls 等 命令 ， 束 是 位 
于 /bi 目录 下 的 可 执行 文件 。 而 对 于 其 他 地 方 的 可 执行 程序 ， 只 要 它 有 
可 执行 权限 ， 那 么 当 我 们 在 终 关中 输入 程序 名 时 ， 它 束 会 运行 。 在 
C++ 编程 时 ， 我 们 像 下 面 这 样 用 编 详 项 把 一 个 文本 文件 编 详 成 可 执行 程 
FF 


slambook/ch2/helloSLAM.cpp 


#include <iostream> 


using namespace std; 


int main( int argc, char** argv ) 


s It 
cout<<"Hello SLAM!"««end1; 
return 0; 


) 





这 是 一 个 非 第 简单 的 程序 。 你 应 该 能 坚 不 费 力 地 看 收 它 ， 所 以 这 里 
不 多 加 解释 一 一 如 果实 际 情况 不 是 这 样 ， 请 你 先 补习 一 下 C++ 的 基本 知 
识 。 这 个 程序 只 是 把 一 个 字符 串 输出 到 屏 右 上 而 已。 你 可 以 用 文本 编辑 
argedit (或 Vim， 如 果 你 在 上 一 讲学 习 了 Vim) 输入 这 些 代 码 ， 并 你 存 
在 上 和 面 列 出 的 路 人 笃 下 。 现 在 ， 我 们 用 编译 占 gt++ (g++ 是 一 个 C++ 编 译 
ay) 把 它 编译 成 一 个 可 执行 文件 。 输 入 : 


1 | g++ helloSLAM.cpp 


"n JUR), IAS AIA IEH. WIL EU 
现 “command not found” 的 错误 信息 ， 说 明 你 可 能 还 没有 安装 g++， 请 用 
如 下 命令 进行 安装 : 


1 | sudo apt-get install g++ 


如 有 果 出 现 列 的 钳 误 ， 请 再 检查 一 过 了 刚才 的 程序 是 含 输入 正确 。 

刚才 这 条 编译 命令 把 helloSLAM.cpp 这 个 文本 文件 编译 成 了 一 个 可 
执行 程序 。 我 们 检 奉 当前 目录 ， 会 发 现 多 了 一 个 a.out 文 件 ， 而 且 它 具有 
执行 权限 (终端 里 颜色 不 同 ) 。 我 们 输入 Ja.out 即 可 运行 此 程序 : 


LIA ./A.büt 
2 | Hello SLAM! 


如 我 们 所 想 ， 这 个 程序 输出 “Hello SLAM!”， 告 诉 我 们 它 在 正确 运 


请 回顾 一 下 我 们 之 前 做 的 事情 。 在 这 个 例子 中 ， 我 们 用 编辑 如 输入 
了 helloSLAM.cpp 的 产 代 人 码 ， 然 后 调用 g++ 编 详 融 对 它 进行 编译 ， 得 到 了 
可 执行 文件 。g++ 责 认 把 源 文件 编译 成 a.out 这 个 名 字 的 程序 (里 然 有 些 
古怪 ,但 十 可 以 接受 的 ) 。 如 打 我 们 愿意 ， 也 可 以 指定 这 个 输出 的 文件 
A EJB) 。 这 古 一 个 极其 人 简 里 的 例子 ， 我 们 使 用 了 大 量 的 默认 参 
数 ， 几 乎 省 略 了 所 有 中 间 步 怠 ， 为 的 古 给 读者 一 个 简洁 的 印象 (里 然 你 
可 能 没有 体会 到 ) 。 下 面 我 们 要 用 cmake 来 编 详 这 个 程序 。 


2.4.3 ”使 用 cmake 


理论 上 说 ， 任 蕙 一 个 C++ 程序 都 可 以 用 g++ 来 编译 。 但 当 程 序 规模 
越 来 越 大 时 ， 一 个 工程 可 能 有 许多 个 文件 夹 和 源 文 件 ， 这 时 输入 的 编译 
命令 将 越 来 越 长 。 通 常 一 个 小 型 C++ 项 目 可 能 含有 十 几 个 类 ， 各 类 间 还 
存在 独 复 杂 的 依 顿 关 系 。 其 中 一 部 分 要 编 详 成 可 执行 文件 ， 另 一 部 分 编 
详 成 库 文 件 。 如 果 仅 靠 g++ 命 令 ， 我 们 需要 输入 大 量 的 编 详 指令 ， 整 个 
编译 过 程 会 变 得 异 香 烦 玉 。 因 此 ， 对 于 C++ 项 目 ， 使 用 一 些 工 程 管 理工 
具 会 更 加 高 效 。 在 历史 上 工程 师 们 曾 使 用 make fi le 进行 目 动 编译 ， 但 下 
面 要 谈 的 cmake 比 它 更 加 方便 。 并 且 ， 我 们 会 看 到 后 面 提 到 的 大 多 数 库 
都 使 用 cmake 来 管理 源 代 码 。 

在 一 个 cmake 工 程 中 ， 我 们 会 用 cmake 命 令 生成 一 个 make fi lex 
件 ， 然 后 ， 用 make 命 令 根 据 这 个 make fi le 文件 的 内 容 编译 整个 工程 。 读 
者 可 能 还 不 知道 make fi le 是 什么 东西 ， 不 过 没关系 ， 我 们 会 通过 例子 来 
尝 习 。 人 仍然 以 上 面 的 helloSLAM.cpp 为 例 ， 这 次 我 们 不 是 直接 使 用 g++， 
而 是 用 cmake 来 制作 一 个 工程 ， 然 后 再 编译 它 。 在 slambook/ch2/ 中 新 建 
一 个 CMakeLists.txt 文 件 ， 内 容 如 下 : 


中 slambook/ch2/CMakeLists.txt 


# 声明 要 求 的 cmake 最 低 版 本 
cmake minimum required( VERSION 2.8 ) 


# 声明 一 个 cmake 工程 
s |project( HelloSLAM ) 


# 添加 一 个 可 执行 程序 
# 语法 ; add executable( 程序 名 源 代码 文件 ) 
add_executable( helloSLAM helloSLAM.cpp ) 





CMakeLists.txt 文 件 用 于 告诉 cmake 我 们 要 对 这 个 目录 下 的 文件 做 什 
么 事情 。CMake-Lists.txt 文 件 的 内 容 需 要 如 守 cmake 有 的 语法 。 这 个 示例 
中 ， 我 们 演示 了 最 基本 的 工程 指定 一 个 工程 名 和 一 个 可 执行 程序 。 根 
据 注 释 ， 旋 者 应 该 理解 每 句 话 做 了 些 什么 。 


现在 ， 在 当前 目录 下 Cslambook/ch2/) ， 调 用 cmake 对 该 工程 进行 


cmake 会 输出 一 些 编译 信息 ， 然 后 在 当前 目录 下 生成 一 些 中 间 文 
件 ， 其 中 最 重要 的 束 是 MakeFile!'1 。 由 于 MakeFile 是 自动 生成 有 的， 我 们 
不 必修 改 它 。 现 在 ， 用 make 命 令 对 工程 进行 编 详 : 


^ make 


scanning dependencies of target helloSLAM 


[100%] Building CXX object CMakeFiles/helloSLAM.dir/helloSLAM.cpp.o 
Linking CXX executable helloSLAM 
[100%] Built target helloSLAM 





编译 过 程 中 会 输出 一 个 编译 进度 。 如 果 顺 利通 过 ， 我 们 或 可 以 得 到 
在 CMakeLists.txt 中 声明 的 那个 可 执行 程序 helloSLAM。 执 行 它 : 


1 |% ./helloSLAM 
2 | Hello SLAM! 
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make 的 做 法 ，cmake 过 程 处 理 了 工程 文件 之 间 的 关系 ， 而 make 过 程 实际 
调用 了 g++ 来 编 详 程序 。 虽 然 这 个 过 程 中 多 了 调用 cmake 和 make 的 步 
又 ， 但 我 们 对 项 目的 编译 管理 工作 ， 从 输入 一 串 g++ 命 令 ， 变 成 了 维护 
和 石 干 个 比较 直观 的 CMakeLists.txt 文 件 ， 这 将 明 最 降低 维护 整个 工程 的 难 
上 度 。 比 如 ， 如 果 想 新 增 一 个 可 执行 文件 ， 只 需 在 CMakeLists.txt 中 添加 一 
行 “add_executable” 命 令 即 可 ， 而 后 续 的 步骤 是 不 变 的 。cmake 会 帮 有 我们 
解决 代码 的 依赖 关系， 而 无 须 输入 一 大 串 g++ 命 令 。 

现在 这 个 过 程 中 唯一 让 我 们 不 满 的 是 ，cmake 生 成 的 中 间 文 件 还 留 
在 我 们 的 代码 文件 当中 。 当 想 要 发 布 代 但 时 ， 我 们 并 不 布 望 把 这 些 中 间 
文件 一 同 发 布 出 去 。 这 时 我 们 还 需要 把 它们 一 个 个 删除 ， 十 分 不 便 。 一 


种 更 好 的 做 法 是 让 这 些 中 间 文 件 都 放 在 一 个 中 间 目 孙 中 ， 在 编译 成 功 
后 ， 把 这 个 中 间 目 录 删 除 即 可 。 上 所 以 ， 更 彰 见 的 编译 cmake 工 程 的 做 法 
如 下 : 


1 | mkdir build 
2 |cd build 
3 |cmake .. 


4 | make 





我 们 新 建 了 一 个 中 间 文 件 夹 “build”， 然 后 进入 build 文 件 夹 ， 通 过 
cmake.. 命 令 对 上 一 层 文 件 夹 ， 也 就 是 代码 所 在 的 文件 夹 进 行 编译 。 这 
样 ，cmake 产 生 的 中 同文 件 束 会 生成 在 build 文 件 夹 中 ， 与 源 代码 分 开 。 
当 故 布 源 代码 时 ， 只 要 把 build 文 件 来 删 挥 即 可 。 请 读者 目 行 按照 这 种 方 
式 对 ch2 中 的 代码 进行 编译 ， 然 后 调用 生成 的 可 执行 程序 (请 记得 把 上 
一 步 产 生 的 中 间 文 件 删 挤 〉。 


2.4.4 {FA 


在 一 个 C++ 工 程 中 ， 并 不 是 所 有 代 公 部 会 编译 成 可 执行 文件 。 只 有 
市 有 main 纯 数 的 文件 才 会 生成 可 执行 程序 。 而 万 一 些 代 码 ， 我 们 只 想 把 
它们 打包 成 一 个 东西 ， 供 其 他 程序 调用 。 这 个 东西 叫 作 库 。 

一 个 库 往往 是 许多 算法 、 程 序 的 集合 ， 我 们 会 在 之 后 的 练习 中 接触 
到 许多 库 。 例 如 ，OpenCV 库 提供 了 许多 计算 机 视觉 相关 的 算法 ， 而 
Eigen 库 近代 了 窍 阵 代 数 的 计算 。 因 此 ， 我 们 要 学 习 如 何 用 cmake 生 成 
库 ， 并 且 使 用 库 中 的 函数 。 现 在 书写 如 下 libHelloSLAM.cpp 文 件 。 


四 slambook/ch2/libHelloSLAM.cpp 


// 这 是 一 个 库 文件 
#include <iostream> 


using namespace std; 


void printHello() 
1 

cout««"Hello SLAM"««endl; 
} 





这 个 库 提 供 了 一 个 printHello 函 数 ， 调 用 此 函数 将 输出 一 条 信息 。 但 
是 它 疫 有 main 函 数 ， 这 意味 着 这 个 库 中 没有 可 执行 文件 。 我 们 在 
CMakeLists.txt 里 加 上 如 下 内 容 : 


1 | add, library( hello libHelloSLAM.cpp ) 


这 条 命令 告诉 cmake， 我 们 想 把 这 个 文件 编译 成 一 个 叫 作 “hello” 的 
库 。 然 后 ， 和 上 面 一 样 ， 使 用 cmake 编 译 整 个 工程 : 


1 | cd build 
2 |cmake .. 
3 |make 


AY, fEbuildoc fF3€ rp3t ^E |, — | libhello.aX-f/F, te RTE 
到 的 库 。 


在 Linux 中 ， 库 文件 分 成 静态 库 和 共 圣 库 两 种 中 S RAS EV af EA 
ERA. KFE Nso. MAER ES ERAH ARR, ARE 
于 议 态 库 每 次 被 调用 部 会 生成 一 个 副本 ， 而 共 圣 库 则 只 有 一 个 副本 ， 
更 省 空间 。 如 果 想 生成 共 圣 库 而 不 是 静态 库 ， 只 需 使 用 以 下 语句 即 可 。 


1 | add library( hello, shared SHARED libHelloSLAM.cpp ) 


r 
, 
上 
上 
. 
, 
上 
, 
" 
r 
a 
r 
L 
^ 


此 时 得 到 的 文件 天 是 libhello_shared.so 了 。 
库 文 件 是 一 个 压 迪 包 ， 里 面 有 编译 好 的 二 进 制 函 数 。 不 过 ， 如 果 仪 


有 .a 或 .so 库 文 件 ， 那 么 我 们 并 不 知道 里 面 的 函数 到 撒 是 什么 ， 调 用 的 形 
式 义 是 什么 样 。 为 了 让 别人 或 者 目 己 ) 使 用 这 个 库 ， 我 们 需要 提供 一 
个 头 文 件 “， 说 明 这 些 库 里 都 有 些 什 么 。 因 此 ， 对 于 库 的 使 用 者 ， 只 要 
拿 到 了 头 文 件 和 库 文 件 ， 残 可 以 调用 这 个 库 了 。 下 面 编 号 libhello 的 头 
Xf. 

^] slambook/ch2/libHelloSLAM.h 


#ifndef LIBHELLOSLAM_H_ 
#define LIBHELLOSLAM_H_ 


3 | void printHello() ; 
#Hendif 





XE, AHEAD CPA Si PE FS BY EOC, D5 
printHello Zi f. RIS —^4 n] REAP OR Wa H A f8] LETS PR: 


Kn slambook/ch2/useHello.cpp 
include "libHelloSLAM.h" 


int main( int argc, char** argv ) 


i 


printHello() ; 


return 0; 





IR Ja, fECMakeLists.txt P Ø IA HT A43 REP AE Ep» BEBE 
刚才 使 用 的 库 上 : 


1 |add_executable( useHello useHello.cpp ) 
2 | target link libraries( useHello hello shared ) 


—— Haj, useHellof£ HF 3l gei] fi Hj hello. shared P F SC 
fa 7 个 小 例子 演示 了 如 何 生 成 并 调用 一 个 库 。 请 注意 ， 对 于 他 人 提 
供 的 库 ， » RAAT AEM GUN 们 进行 调用 ， 整 合 到 自己 的 程序 
cH 


除了 已 经 演示 的 功能 之 外 ，cmake 还 有 许多 语法 和 选项 ， 这 里 不 一 
一 列举 。 习 题 中 包 舍 了 一 些 cmake 的 阅读 材料 ， 感 兴趣 的 读者 可 目 行 阅 
谈 。 现 在 ， 徐 单 问 顾 一 下 我 们 之 前 做 了 哪些 事 : 


1. 自 完 ， 和 程序 代 公 由 尖 文 件 和 源 文 件 组 成 。 


2. 市 有 main 函 数 的 源 文件 编 详 成 可 执行 程序 ， 其 他 的 编 详 成 库 文 
件 。 


3. 如 朱 可 执 行程 序 力 调 用 库 文 件 中 的 函数 ， 它 需要 参考 蒋 库 皖 供 的 
头 文 件 ， 以 明日 调用 的 格式 。 同 时 ， 要 把 可 执行 程序 链接 人 到 库 文件 上 。 


这 几 个 步 又 应 该 是 简 早 清楚 的 ， 但 实际 操作 中 你 可 能 会 过 上 一 些 问 
题 。 比 如 说 ， 如 果 代 但 里 引用 了 库 的 函数 ， 但 筷 了 把 程序 链接 到 库 上 ， 
会 发 后 什 么 昵 ?请 试 试 把 CMake-Lists.txt 中 的 链接 部 分 去 扩 ， 看 看 会 发 
FEAF Ata. RAE S cmakefk £i PIE V TH JUL ? 


2.4.5 ”使 用 IDE 


最 后 ， 我 们 来 谈 谈 如 何 使 用 集成 开发 环境 (Integrated. Development 
Environment，IDE)〉。 前 和 面 的 编程 完全 可 以 用 一 个 简 捍 的 文本 编辑 器 来 
完成 。 然 而 ， 你 可 能 需要 在 各 个 文件 间 跳 来 跳 去 ， 人 查询 某 个 函数 的 声明 
和 实现 。 当 文件 很 多 时 ， 这 仍然 很 烦琐 。IDE 为 开发 者 提供 了 跳 转 、 补 
全 、 断 点 调试 等 很 多 方便 的 功能 ， 所 以 ， 我 们 建议 谈 者 选择 一 个 IDE 进 
ITHE o- 

Linux 下 的 IDE 有 很 多 种 。 虽 然 与 Windows 下 的 Visual Studio 还 有 一 
些 甜 距 ， 不 过 文 持 C++ 开 有 友 的 也 有 好 几 种 ， 例 如 : Eclipse、QtCreator、 
Code::Blocks、Clion， 等 等 。 同 样 ， 我 们 不 强制 谈 者 使 用 采种 特定 的 
IDE， 而 仅 给 出 我 们 的 建议 。 我 们 使 用 的 是 Kdevelop〈 见 图 2-12) . € 
是 一 个 免费 软件 ， 在 Ubuntu 的 源 中 提供 了 ， 这 和 意味 看 你 可 以 用 apt-get 来 
T Eo Kdevelopi i ra 912 UH F: 


1.x EEcmake LE. 


2. 对 C++ 文 持 较 好 (包括 11 标 准 ) 。 有 高 亮 、 跳 转 、 补 全 等 功能 。 
He H ARMAS o 


3. 能 方便 地 看 到 各 个 文件 和 目录 树 。 
4. 有 一 键 编 译 、 断 点 调试 等 功能 。 
5.76 AV] $3 e 


default: ch2 - [ ch2/libHelloSLAM.cpp ] ~ KDevelop 
>b 构建 {O Execute je Debug & CJ » @ +| printHello() 


€ S CMakeListstxt € | libHelloSLAM.h € libHelloSLAM.cpp X | useHello.cpp 其 
#include <iostream> 
using namespace std; 


void printHello() 
mt 
cout««"Hello SLAM"««end1;ll 


C? helloSLAM.cpp ) 


G libHelloSLAM.cpp 
b libHelloSLAM.h 
C? useHello.cpp 


(0 0-410U hb UN HP 


zw Do @@ OP 


x a d» 国 [ 5 | mam 


il been written to: /home/xiang/slambook/ch2/build 
[ 25%] [ 50%] Bullt target hello 

Built target helloSL 

[ 7596] Built target hello shared 

Scanning dependencies of target useHello 

[10096] Building OC object CMakeFiles/useHello.dir/useHello.cpp.o 
Linking CXX executable useHello 

[10096] Bullt target useHello 

et 已 完成 tit 





下 构建 | 外 问题 OKBWAB 


图 2-12  Kdevelop 7 [f . 


基本 上 ， 我 们 对 一 个 IDE 的 功能 要 求 它 都 具备 ， 所 以 读者 不 妨 尝试 
一 下 。 有 时 候 你 会 碰 到 一 些 问 题 ， 例 如 对 某 些 模板 类 的 解析 响应 比较 组 
侵 等 ， 确 实 它 还 不 够 完善 。 不 过 相 比 于 其 他 IDE， 它 是 不 错 的 。 


Kdevelop 原 生 文 持 cmake 工 程 。 有 具体 做 法 是 ， 在 终 妆 建立 
CMakeLists.txt 后 ， 用 Kdevelop 中 的 “工程 > 打开 /导入 工程 ”打开 
CMakeLists.txt。 软 件 会 询问 你 几 个 问题 ， 并 且 默 认 建立 一 个 build 文 件 
夹 ， 帮 你 调用 刚才 的 cmake 和 make 命 令 。 只 要 按 下 快捷 键 F8， 这 些 都 可 
以 目 动 完 成 。 图 2-12 的 下 和 面部 分 束 显 示 了 编译 信息 。 


我 们 把 适应 IDE 的 任务 交 给 谈 者 目 己 来 完成 ， 而 并 不 打算 在 书 中 进 
ITEM. GRR EM Windows WORN, eee eM FS Visual 
C++ 或 Visual Studio 挺 相 似 。 请 用 Kdevelop 打 开 刚 才 的 工程 然后 进行 编 
详 ， 看 看 它 输出 什么 信息 。 相 信和 你 会 觉得 比 打开 终 闪 更 方便 一 些 。 


不 过 ， 本 节 重 点 想 讲 的 是 如 何在 IDE 中 进行 调试 。 在 Windows 下 编 
程 的 同学 多 半 会 有 在 Visual Studio 下 断 上 点 调试 的 经 历 。 不 过 在 Linux 中 ， 
默认 的 调试 工具 gdb 只 提供 了 文本 界面 ， 对 新 手 来 讲 不 太 方 便 。 有 些 IDE 
提供 了 上 断 点 调试 功能 《〈 撒 层 仍 旧 是 gdb) ，Kdevelop 束 是 其 中 之 一 。 要 
使 用 Kdevelop 的 断 点 调试 功能 ， 你 需要 完成 以 下 几 件 事 : 


1. 在 CMakeLists.txt 中 把 工程 调 为 Debug 编 译 模 式 。 


2. 千 诉 Kdevelop 你 想 运 行 哪个 程序 。 如 果 有 参数 ， 也 要 配置 它 的 参 
数 和 工作 目录 。 


3. 进 入 断 点 调试 界面 ， 瓯 可 以 单 步 运行 ， 看 到 中 间 变 量 的 人 了 。 
第 一 步 ， 在 CMakeLists.txt 中 加 入 下 面 的 命令 来 设置 编 详 模式 : 


1 | set( CMAKE BUILD TYPE "Debug" ) 


cmake 目 市 一 些 编译 相关 的 内 部 变量 ， 它 们 可 以 对 编 详 过程 进 行 更 
精细 的 控制 。 对 于 编译 类 型 ， 通 第 有 调试 用 的 Debug 模 式 与 发 布 用 的 
Release 模 式 。 在 Debug 模 式 中 ， 程 序 运 行 较 慢 ， 但 可 以 进行 断 点 调试 ; 
而 Release 模 式 则 速度 较 快 ， 但 没有 调试 信息 。 我 们 把 程序 设置 成 Debug 
模式 ， 吏 能 放置 断 点 了 。 接 下 来 ， 告 诉 Kdevelop 你 想 局 动 哪个 程序 。 


Bow, Fei Aaa”, We BEAN “Add New 
应 用 程序 ”。 在 这 一 步 中 ， 我 们 的 任务 是 告诉 Kdevelop 想 要 局 动 哪 一 个 
程序 。 如 图 2-13 所 示 ， 既 可 以 直接 选择 一 个 cmake 的 工程 目标 (也 就 古 
我 们 用 add_executable 指 令 构 建 的 可 执行 程序 ) ， 也 可 以 直接 指 同 一 个 
二 进 制 文件 。 建 议 使 用 第 三 各 方式， 根据 我 们 的 经 验 ， 这 样 更 少 出 现 问 


e o 


+ Add New..»| = Remove Selected Editing 应 用 程序 : 新 建 应 用 程序 启动 器 
全 局 可 执行 文件 
Sà ch2 工程 目标 : 6 och2/useHello 
- — dus 可 执行 文件 : 
行为 

参数 : 

工作 目录 : 

环境 : default 

使 用 外 部 终端 : 


依赖 
动作 : | 无 动作 


目标 : 





图 2-13  JHz AR X EL IBI e 


在 第 二 栏 里 ， 可 以 设置 程序 的 运行 参数 和 工作 目录 。 有 了 时 程序 是 有 
运行 参数 的 ， 它 们 会 作为 main 孙 数 有 的 参数 被 传 入 。 如 有 果 没 有 则 可 以 留 
空 ， 对 于 工作 目录 亦 是 如 此 。 配 置 好 这 两 项 后 ， 可 以 单 击 “OK2” 按 钮 保 
存 配 置 结 果 。 


刚才 这 几 步 我 们 配置 了 一 个 应 用 程序 的 局 动 项 。 对 于 每 一 个 局 动 
项 ， 我 们 可 以 单 击 “EExecute” 按 钮 直接 月 动 这 个 程序 ， 也 可 单 
击 “Debug” 按 钮 对 它 进 行 晰 点 调试 。 读 者 可 以 试 着 单 击 “Execute” 投 钮 ， 
伍 看 输出 的 结果 。 现 在 ， 为 了 调试 这 个 程序 ， 单 击 printHello 那 行 的 左 
侧 ， 增 加 一 个 断 点 。 然 后 ， 单 击 *Debug” 按 钮 ， 程 序 会 停留 在 断 点 处 等 
待 ， 如 图 2-14 所 示 。 


default: ch2 -[ ch2/useHello.cpp ] - KDevelop 
n & 调试 。 or 构建 9) Execute (Debug Q 全 部 停止 Q Fike » 全 | v | main(int, char**) ? (>) 继续 » 


LI 
"IE € 5 (CMakeListstxt € libHelloSLAM.h 3€ libHelloSLAM.cpp 3€ | useHello.cpp 3€ 行 :5 列 :1 vi 
E x 
$5 名 称 值 1  £include "LibHelloSLAM.h" it 
自动 2 
S " 局 部 变量 3 int main( int argc, char** argv ) 
m argc 1 4 w( 
x > argv Ox7fffffffesos oe 5 | printHello(); 
= 6 return 0; 
了 } M 
£ 5 do ga 
新 建 应 用 程序 启动 器 


13 
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Xii | 名 运行 1928 框架 堆栈 器 GDB OS 


图 2-14 ”调试 界面 。 

调试 时 ，Kdevelop 会 切换 到 调试 模式 ， 界 面 会 发 生 一 点 变化 。 在 断 
点 处 ， 可 以 用 单 步 运行 (F10 键 〉”、 单 步 跟 进 (F11 键 )、 单 步 跳 出 
(F12 键 ) 功能 控制 程序 的 运行 。 同 时 ， 可 以 点 开 左 侧 的 界面 ， 查 看 局 
部 变量 的 值 。 或 者 选择 “ 候 止 按钮， 结束 调试 。 调 试 结束 后 ，Kdevelop 
会 回 到 正常 的 开发 界面 。 

现在 你 应 该 熟悉 了 整个 断 点 调试 的 流程 。 今 后 ， 如 果 在 程序 运行 阶 
段 肥 生 了 错误 ， 导 致 程 序 朋 吝 ， 束 可 以 用 断 点 调试 确定 出 错 的 位 置 ， 然 
后 加 以 修正 上 。 

习题 

1. 阅 读 文 献 [1] 和 [14]， 你 能 看 懂 其 中 的 内 容 吗 ? 

2.* 阅 读 SLAM 的 综述 文献 ， 例 如 [9,15,16,17,18] 等 。 这 些 文献 关于 
SLAM 的 看 法 与 本 书 有 何 异 同 ? 

3.g++ 命 令 有 哪些 参数 ? 怎么 填写 参数 可 以 更 改 生 成 的 程序 文件 
PA, 

4. 使 用 build 文 件 夹 来 编译 你 的 cmake 工 程 ， 然 后 在 Kdevelop 中 试 
试 。 

5. 刻 意 在 代码 中 添加 一 些 语法 错误 ， 看 看 编译 会 生成 什么 样 的 信 


已 。 你 能 看 全 g++ 的 错误 信息 吗 ? 
6. 如 条 所 了 把 库 链 接 到 可 执行 程序 上 ， 编 译 会 报错 吗 ? 报 什么 样 的 


TH 2 


7.* [aise €cmakeSZEE) , J fffemakel] Hz EX. 


8.* 完 善 hello SLAM 小 程序 ， 把 它 做 成 一 个 小 程序 库 ， 安 装 到 本 地 
便 盘 中 。 然 后 ， 新 建 一 个 工程 ， 使 用 吾 nd_package 找 这 个 库 并 调用 。 


9.* 桔 找 其 他 cmake 教 学 材料 ， 深 入 了 解 cmake， 例 如 
https://github.com/TheErk/CMake-tutorial 。 


10. 找 到 Kdevelop 的 官方 网 站 ， 看 看 它 还 有 哪些 特性 。 你 都 用 上 了 
li ? 


11. 如 果 在 上 一 讲学 习 了 Vim， 请 试 试 Kdevelop 的 Vim 编 辑 功 能 。 





[1] 不 过 现实 中 我 们 都 会 有 一 个 大 概 的 范围 ， 例 如 室内 和 室外 的 区 分 。 

[2] 男 成 单 目 会 比较 吓人 。 

[3] 数学 上 的 原因 将 会 在 视觉 里 程 计 一 讲 中 解释 。 

[4] 你 可 以 用 手机 录 个 小 视频 试 试 。 

[5] BAW RP A Ja vim (Back End) 。 由 于 主要 使 用 的 是 优化 方法 ， 故 称 为 后 端 优化 。 
[6] https://en.wikipedia.org/wiki/A* search algorithm 

[7] 在 本 书 中 ， 我 们 以 “位 姿 ” 这 个 词 表 示 “ 位 置 " 加 上 “姿态 ”。 


[8] 我 们 以 后 称 它 为 位 姿 (Pose) ， 以 与 位 置 进 行 区 别 。 我 们 说 的 位 姿 ， 包 含 了 旋转 
(Rotation) 和 平移 (Translation) 。 


[9] 感谢 TUNA 同 学 们 的 维护 ! 
[10] 大 多 数 人 第 一 次 看 到 Ubuntu 都 觉得 很 漂亮 。 


[11] MakeFile 是 一 个 自动 化 编译 的 脚本 ， 读 者 现在 可 以 将 它 理解 成 一 系统 自动 生成 的 编译 指 
令 ， 而 无 须 理会 其 内 容 。 


[12] 你 多 半 猜 错 了 ， 它 并 不 叫 作 动态 库 。 
[13] 而 个 是 下 接 给 我 们 发 邮件 询问 怎么 处 理 遇 到 的 问题 。 
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在 上 一 讲 中 ， 我 们 讲解 了 视觉 SLAM 的 框架 与 内 容 。 本 讲 将 介绍 视 
觉 SLAM 的 基本 问题 之 一 : 一 个 刚体 在 三 维 空间 中 的 运动 是 如 何 摘 述 的 
。 我 们 当然 知道 这 由 一 次 旋转 加 一 次 平移 组 成 。 平 移 确 实 没有 太 大 问 
el, (A eRe Ae ERIS. BOT DRT ZA ee FERRE. WB. WR ALAA 
的 意义 ， 以 及 它们 是 如 何 运 算 和 转换 的 。 在 实践 部 分 ， 我 们 将 介绍 线性 
代数 库 Eigen。 它 提供 了 C++ 中 的 矩阵 运算 ， 并 有 日 它 的 Geometry 模 块 还 所 
供 了 四 元 数 等 刚体 运动 的 描述 。Eigen 的 优化 非常 完善 ， 但 是 它 的 使 用 
方法 有 一 些 特殊 的 地 方 ， 我 们 会 在 程序 中 介绍 。 





3.1 旋转 矩阵 


3.1.1 AMEE, AP bg f 


我 们 日 第 生活 的 空间 是 三 维 的 ， 因 此 我 们 生来 就 习惯 于 三 维 空 间 的 
运动 。 三 维 空间 由 3 个 轴 组 成 ， 所 以 一 个 空间 点 的 位 置 可 以 由 3 个 坐标 指 
定 。 不 过 ， 我 们 现在 要 考虑 刚体 ”， 它 不 光 有 人 位置， 还 有 目 里 的 姿态 。 
相机 也 可 以 看 成 三 维 空 间 的 刚体 ， 于 是 位 置 古 指 相 机 在 空间 中 的 哪个 地 
方 ， 而 姿态 则 是 指 相机 的 瑚 回 。 结 合 起 来 ， 我 们 可 以 说 ,，“ 相 机 正 处 于 
空间 (0，0，0) 点 处 ， 瑚 问 正 前 方 ”* 这 样 的 话 。 但 是 这 种 目 然 语言 很 烦 玉 ， 
我 们 更 喜欢 用 数学 语言 来 摘 述 它 。 

我 们 从 最 基本 的 内 容 讲 起 : 点 ”和 向 量 ” 。 点 的 几何 意义 很 容易 理 
fo PEETA? 它 是 线性 空间 中 的 一 个 元 系 ， 可 以 把 它 想 象 成 从 原 
点 指 癌 某 处 的 一 个 葡 头 。 需 要 提醒 读者 的 是 ， 请 不 要 把 同 量 与 它 的 坐标 
两 个 概念 寓 消 。 一 个 同 量 是 空间 当中 的 一 样 东 西 ， 比 如 说 a . xxHa 并 
不 是 和 香干 个 实数 相关 联 的 。 只 有 当 我 们 指定 这 个 三 维 空间 中 的 荣 个 坐 
标 系 ” 时 ， 才 可 以 谈论 该 癌 量 在 此 坐标 系 下 的 坐标 ， 也 就 是 找到 行 干 个 
实数 对 应 这 个 同 量 。 人 例如， 三维 空间 中 的 荣 个 回 量 的 坐标 可 以 用 Ra 当 
中 的 3 个 数 来 摘 述 。 某 个 点 的 坐标 也 可 以 用 Ra 来 描述 。 怎 么 描述 呢 ? 如 
果 我 们 确定 了 一 个 坐标 系 ， 也 就 是 一 个 线性 空间 的 基 (e e, e) WA 
可 以 谈论 问 量 a 在 这 组 基 下 的 坐标 了 : 


a= lei, €», €3| do = Q1€1 十 Qoeo + a3€3. (3.1) 


所 以 坐标 的 其 体 取 值 ， 一 是 和 问 量 本 里 有 关 ， 二 是 和 坐标 系 的 选取 
有 关 。 坐 标 系 通常 由 3 个 正 交 的 坐标 轴 组 成 《尽管 也 可 以 有 非 正 交 的 ， 
但 实际 中 很 少见 ) 。 例 如 ， 给 定 x 和 y 轴 时 ，z 轴 就 可 以 通过 右手 《或 左 


TO 法 则 由 xxy 定义 出 来 。 根 据 定 义 方式 的 不 同 ， 坐 标 系 义 分 为 左手 系 
和 右手 系 。 左 手 系 的 第 3 个 轴 与 右手 系 方 同 相 有 反 。 束 经 验 来 讲 ， 人 们 更 
习惯 使 用 右手 系 ， 尽 管 也 有 一 部 分 程序 库 仍 使 用 左手 系 。 

根据 基本 的 线性 代数 知识 ， 我 们 可 以 谈论 问 量 与 同 量 ， 以 及 问 量 与 
数 之 间 的 运算 ， 例 如 数 乘 、 加 法 、 减 法 、 内 积 、 外 积 等 。 数 乘 和 四 则 运 
算 都 是 相当 基本 的 内 容 ， 这 里 不 再 疆 述 。 内 外 积 对 读者 来 说 可 能 有 些 阳 
生 ， 这 里 给 出 它们 的 运算 方式 。 对 于 ape Rs ， 内 积 可 以 写成 : 


a-b=a'b= 3 aibi = |a| |b| cos (a, b) . (3,2) 


=l 


内 积 可 以 手 述 问 量 间 的 投影 关系 。 而 外 积 则 是 这 个 样子 : 


1 J k abs = 030» 0 — 3 a9 
A 
axb= a; ag Q3 = | a3bı 一 Q103 = Q3 0 —1 b — a^b. (3.3) 
bi b» bs Q0» EE ab, — 2 Q1 ( 
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量 张 成 的 四 边 形 的 有 同 面 积 。 对 于 外 积 ， 我 们 引入 了 AS, da 写成 
一 个 矩阵 。 事 实 上 是 一 个 反对 称 窍 阵 (Skew-symmetric) ， 你 可 以 将 ^ 
记 成 一 个 反对 称 符 号 。 这 样 就 把 外 积 axb 写成 了 和 矩阵 与 向 量 的 乘法 a^ b 
， 把 它 变 成 了 线性 运算 。 这 个 符号 将 在 后 文 经 常用 到 ， 请 记 住 它 。 外 积 
只 对 三 维 辣 量 存在 定义 ， 我 们 还 能 用 外 积 表 示 问 量 的 旋转 。 

为 什么 外 积 可 以 表示 旋转 呢 ? 

考虑 两 个 不 平行 的 同 量 abp ， 我 们 要 描述 从 a 到 b 之 间 是 如 何 旋转 
的 ， 如 图 3-1 所 示 。 我 们 可 以 用 一 个 同 量 来 拉 述 三 维 空间 中 两 个 同 量 的 
旋转 关系 。 在 右手 法 则 下 ， 我 们 用 右手 的 4 个 指头 从 a 转 问 b ， 大 拇指 朝 
回 就 是 旋转 回 量 的 方向 ， 事 实 上 也 是 axp 的 方向 。 它 的 大 小 则 由 oa Mb 
的 夹 角 决定 。 通 过 这 种 方式 ， 我 们 构造 了 从 a 到 b 的 一 个 旋转 同 量 。 这 
个 向 量 同样 位 于 三 维 空间 中 ， 在 此 坐标 系 下 ， 可 以 用 3 个 实数 来 描述 。 
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图 3-1 ”左右手 系 的 区 别 与 向量 间 的 旋转 。a 到 的 旋转 可 以 由 癌 量 w 来 描述 。 


3.1.2 ”坐标 系 间 的 欧 氏 变换 


与 同 量 间 的 旋转 类 似 ， 同 样 可 以 手 述 两 个 坐标 系 之 间 的 旋转 天 系 ， 
再 加 上 和 平移， 统称 为 坐标 系 之 间 的 变换 关系。 在 机 需 人 的 运动 过 程 
中 ， 和 见 的 做 法 是 设 定 一 个 惯性 坐标 系 《 或 者 叫 世 界 坐 标 系 ) ， 可 以 认 
为 它 是 固定 不 动 的 ， 例 如 图 3-2 中 的 xw ;yw ,zw 定义 的 坐标 系 。 同 时 ， 相 
机 或 机 如 人 则 是 一 个 移动 坐标 系 ， 例 如 xc ,yc ,zc 定义 的 坐标 系 。 我 们 可 
能 会 问 : 相机 视野 中 某 个 同 量 p ， 它 的 坐标 为 p。 ， 而 在 世界 坐标 系 下 
看 ， 它 的 坐标 p, 。 这 两 个 坐标 之 间 是 如 何 转换 的 呢 ? 这 时 ， 残 需要 先 得 
到 访 点 针对 机 规 人 坐标 系 的 坐标 值 ， 再 根据 机 硕 人 位 姿 转换 到 世界 坐标 
系 中 ， 这 个 转换 关系 由 一 个 滤 隆 T 来 拍 述 ， 如 图 3-2 所 示 。 


LO CE ， 


Xy "15 p 
图 3-2 ”坐标 变换 。 对 于 同一 个 同 量 p ， 它 在 世界 坐标 系 下 的 坐标 p ,和 在 相机 坐标 系 下 的 
坐标 P_ 是 不 同 的 。 这 个 变换 关系 由 坐标 系 间 的 变换 窍 阵 了 来 摘 述 。 





相机 运动 是 一 个 刚体 运动 ， 它 保证 了 同一 个 同 量 在 各 个 坐标 系 下 的 
长 度 和 夹 角 邦 个 会 及 生变 化 。 这 种 变换 称 为 欧 氏 变 撕 。 想 象 你 把 手机 
HEI, EVP ZA, JRA eA T AENA ZI. mE 
自己 的 长 度 、 各 个 面 的 角度 等 性 质 不 会 有 任何 变化 。 这 样 一 个 欧 氏 变换 
由 一 个 旋转 和 一 个 平移 两 部 分 组 成 。 首 先 来 考 碟 旋转 。 我 们 发 菏 个 单位 
正 交 基 (el,ez,es) o D UK TERE Ab py, T ( (e1,€5,€3)0 那么 ， 对 于 同一 个 同 量 
a 《注意 访问 量 并 没有 随 痢 坐标 系 的 旋转 而 发 生 运动 ) ， 它 在 两 个 坐标 
A RAAB N a, a2, a3]? Plai azao RIER EX, A: 


|e, C2， e3] a2 一 lei, e2， e3] a5 à; (3.4) 
a3 a3 


为 了 拍 述 两 个 坐标 之 间 的 关系 ， 我 们 对 上 述 等 式 的 元 右 两 边 同 时 元 


Fe | er |, 那么 元 边 的 系数 融 变 成 了 单位 窃 阵 ， 所 以 : 


C3 
EL. T 4/ Tost / 
A 
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Q9 E €5€| €» €» €5€2 Co m Ra i (3.5) 
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RAIER KE OR, XEOU T ANERER 。 这 个 窍 阵 由 两 组 基 
之 间 的 内 积 组 成 ， 刻 男 了 旋转 前 后 同一 个 同 量 的 坐标 变换 关系 。 只 要 旋 
转 是 一 样 的 ， 那 么 这 个 沧 阵 也 是 一 样 的 。 可 以 说 ， 和 矩阵 R 插 述 了 旋转 本 
身 。 因 此 又 称 为 旋转 窍 阵 。 

旋转 滤 阵 有 一 些 特别 的 性 质 。 事 实 上 ， 它 是 一 个 行列 式 为 1 的 正 交 
REPEU 。 反 之 ， 行 列 却 为 1 的 正 区 矩阵 也 是 一 个 旋转 矩阵 。 所 以 ， 可 以 
把 旋转 矩阵 的 集合 定义 如 下 : 


SO(n) —-[RcR"*"|RR! = I,det(R) = 1). (3.6) 


SO(n ) 是 特殊 正 交 群 (Special Orthogonal Group) 的 意思 。 我 们 把 
解释 “ 群 ”的 内 容留 到 下 一 讲 。 这 个 集合 由 n 维 空间 的 旋转 矩阵 组 成 ， 特 
列 地 ，SO(3) 束 是 三 维 空间 的 旋转 了 了。 通过 旋转 矩阵 ， 我 们 可 以 直接 谈 
论 两 个 坐标 系 之 则 的 旋转 变换 ， 而 不 用 再 从 基 开 始 谈 起 。 换 句 话 说 ， 放 
FEE ME AY DA SHIA AA LAN EFS 。 

HH We Fe Ha BY TE ACFE RE, EAE RE) 摘 述 了 一 个 相反 的 旋 
转 。 按 照 上 面 的 定义 方式 ， 有 : 


a’ = R !a = R'a. (3.7) 
显然 RT 刻画 了 一 个 相反 的 旋转 。 


在 欧 氏 变换 中 ， 际 了 旋转 之 外 还 有 平移 。 考 处世 界 坐 标 系 中 的 问 量 
ZAIRE CAR FHA) 和 一 次 平移 [后 ， 得 到 了 a ， 那 么 把 旋转 


Q 


和 平移 合 一起， 有 : 
a’ = Ra +t. (3.8) 
Ann, t BRACE AS st. THEE T ete. PROD A midi b iE 
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PER 和 一 个 平移 问 量 t 完整 地 拍 述 了 一 个 欧 氏 空间 的 坐标 变换 关系 。 


3.1.3 ZEAE REFERE HR 


I (3.8) 完整 地 表达 了 欧 氏 衬 间 的 旋转 与 平移 ， 不 过 还 存在 一 个 
小 问题 : 这 里 的 变换 关系 不 是 一 个 线性 关系 。 假 设 我 们 进行 了 两 次 变 
Jh. R,,t, MIR, st, » WE: 


b= Ria+ ti, c= Rob + tə. 


但 是 从 a 到 c 的 变换 为 


C — Rə (Ria + ti) T tə. 


这 样 的 形 却 在 变换 多 次 之 后 会 过 于 复杂 。 因 此 ， 我 们 要 引入 齐 次 从 
标 和 变换 窍 阵 重 写 式 (3.8): 
DED 
1 1 


H 
这 是 一 个 数学 技巧 : 我 们 在 一 个 三 维 向 量 的 末尾 添加 1， 将 其 变 成 
了 四 维 回 量 ， 称 为 章 次 坐标 ”。 对 于 这 个 四 维 同 量 ， 我 们 可 以 把 旋转 和 
平移 与 在 一 个 窍 阵 里 面 ， 使 得 整个 关系 变 成 线性 关系 。 访 式 中 ， 和 矩阵 了 
称 为 变换 矩阵 〈 Transform Matrix) 。 我 们 暂时 用 & 表示 a 的 齐 次 坐 
标 。 
稍微 说 一 下 齐 次 坐标 。 它 是 射影 几何 里 的 概念 。 通 过 添加 最 后 一 
维 ， 我 们 用 4 个 实数 捅 述 了 一 个 三 维 同 量 ， 这 显然 多 了 一 个 日 由 上 度 ， 但 
允许 我 们 把 变换 写成 线性 的 形式 。 在 齐 次 坐标 中 ， 菜 个 点 x 的 每 个 分 量 


R t 
of 1 








同 习 一 个 非 零 第 数 k 后 ， 仍 然 表 示 同 一 个 点 。 因 此 ， 一 个 点 的 共 体 坐标 
值 不 是 唯一 的 。 如 [1, 1, 1, 1 和 [2, 2, 2, 2] 是 同一 个 点 。 但 当 最 后 一 项 
不 为 零 时 ， 我 们 总 可 以 把 所 有 坐标 除 以 最 后 一 项 ， 强 制 最 后 一 项 为 1， 
从 而 得 到 一 个 点 唯一 的 坐标 表示 《也 融和 是 转换 成 非 齐 次 坐标 ) : 
£—|[r,y,z,w] = [x/w,y/w,z/w,1] . (3.10) 
XXI, MSP 2 AR p RU RE IE PE TK 
乱 齐 次 坐标 和 变换 窃 阵 ， 两 次 变换 的 累加 融 可 以 有 很 好 的 形 却 : 


~ 


b=T7,4,6=Thb >č=DT à. (3.11) 

但 是 区 分 齐 次 和 非 齐 次 坐标 的 符号 令 我 们 感到 厌烦 。 在 不 引起 到 义 
的 情况 下 ， 以 后 我 们 束 直 接 把 它 写 成 =Ta 的 样子 ， 默 认 其 中 是 齐 次 坐 
标 了 。 


ATAGÉOBERT ， 它 其 有 比较 特别 的 结构 : 左上 角 为 旋转 和 矩阵 ， 碳 
侧 为 平移 同 量 ， 左 下 角 为 0 回 量 ， 右 下 角 为 1。 这 种 窍 阵 又 称 为 特殊 欧 氏 
HE (Special Euclidean Group) : 


R t 
SE(3) — Ir = | : | c R^*^|R e SO(3),t € d (3.12) 
0! 1 
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Tt = (3.13) 
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种 。 例 如 ， 当 我 们 写 To 时 ， 使 用 的 是 齐 识 坐标 《不 然 没 法 计算 ) 。 而 
写 Ra WW, EHETE At. WR STEPS, IESE AL 
by BUI AR IFR, ee CAE Anat i Aa tn IES UR AB ip 
之 加 的 转换 事实 上 非常 容易 。 


回顾 一 下 : 首先 ， 介 绍 了 回 量 及 其 坐标 表示 ， 并 介绍 了 同 量 间 的 运 
$$. 然后 ， 坐 标 系 之 间 有 的 运动 由 网 氏 变 换 手 述 ， 它 由 平移 和 旋转 组 成 。 
旋转 可 以 由 旋转 矩阵 5O(3) 手 述 ， 而 平移 卫 接 由 一 个 RE HEMD. W 
后 ， 如 朵 将 平移 和 旋转 放 在 一 个 矩阵 中 ， 束 形成 了 变换 滤 阵 SE(3)。 


3.2 SCE: Eigen 


本 讲 的 实践 部 分 有 两 五 。 第 一 部 分 中 ， 将 讲解 如 何 使 用 Eigen 来 表 
示 窍 阵 、 回 量 ， 随 后 引申 全 旋转 窍 阵 与 变换 算 阵 的 计算 。 本 而 的 代 但 在 
slambook/ch3/useEigenH 。 


Eigen"! 是 一 个 C++ 开源 线性 代数 库 。 它 提供 了 快速 时 有 关 和 矩阵 的 线 
性 代数 运算 ， 还 包括 解 方程 等 功能 。 许 多 上 层 的 软件 库 也 使 用 Eigen 进 
行 矩 阵 运算 ， 包 括 g20、Sophus 等 。 照 应 本 讲 的 理论 部 分 ， 我 们 来 学 习 
一 下 Eigen 的 编程 。 


你 的 PC 上 可 能 还 没有 安装 Eigen。 请 输入 以 下 命令 进行 安装 : 
. 


大 部 分 第 用 的 库 剖 已 在 Ubuntu 软 件 源 中 提供 。 以 后 ， 右 想 要 安 疙 东 
个 库 ， 不 妨 先 搜索 一 下 Ubuntu 的 软件 源 中 是 任 已 提供 。 通 过 apt 命 令 ， 
我 们 能 够 方便 地 安装 Eigen。 回 顾 上 一 讲 的 知识 ， 我 们 知道 一 个 库 由 头 
文件 和 库 文 件 组 成 。Eigen 尖 文件 的 默认 位 置 在 “/usr/include/eigen3/”* 中 。 
如 果 不 确 定 ， 可 以 输入 以 下 命令 查找 : 


1 | sudo updatedb 
2 |locate eigen3 


相 比 于 其 他 库 ，Eigen 的 特殊 之 处 在 于 ， 它 是 一 个 纯 用 头 文 件 搭 建 
ERRE CXIE! ) 。 这 意味 痢 你 只 能 找到 它 的 头 文 件 ， 而 没 
有 .So 或 .a 那 样 的 二 进 制 文件 。 在 使 用 时 ， 只 需 引 入 Eigen 的 头 文 件 即 可 ， 
不 需要 链接 库 文 件 (因为 它 没 有 库 文件 ) 。 下 面 写 一 段 代 人 码 来 实际 练习 
一 下 Eigen 的 使 用 : 


四 slambook/ch3/useEigen/eigenMatrix.cpp 


#include <iostream> 


#include <ctime> 


using namespace std; 


// Eigen 部 分 
#include <Eigen/Core> 
// PAE FEE KRIS (iR. MEAETR ) 


#include <Eigen/Dense> 


#define MATRIX_SIZE 50 


OGIO IOI ATR II TAK 
* 本 程序 演示 了 Eigen 基本 类 型 的 使 用 


ae ok ok oe eek ke IG I I I ee ek / 


int main( int argc, char** argv ) 


1 


// Eigen 以 矩阵 为 基本 数据 单元 。 它 是 一 个 模板 类 。 它 的 前 三 个 参数 为 : 数据 类 型 ， 行 
列 

// 声明 一 个 2x3 的 float 矩阵 

Eigen: :Matrix<float, 2, 3> matrix 23; 

// PRT, Eigen 通过 typedef 提供 了 许多 内 置 类 型 ， 不 过 底层 仍 是 Eigen: :Matric 
// 例如 Vector3d 实质 上 是 Eigen::Matriz«double, 3, 1> 

Eigen::Vector3d v 3d; 

// 还 有 Matria3d 实质 上 是 Eigen: :Matriz<double, 3, 3> 

Eigen::Matrix3d matrix 33 = Eigen::Matrix3d::Zero(; [初始 化 为 零 

// Te CR HE ABER AG, TAE A h AS AK 4E 

Eigen::Matrix« double, Eigen::Dynamic, Eigen::Dynamic > matrix dynamic; 

// 更 简单 的 

Eigen::MatrixXd matrix x; 


// 这 种 类 型 还 有 很 多 ， 我 们 不 一 一 列举 


// 下 面 是 对 和 矩阵 的 操作 

// 输入 数据 

matrix 253 << 1, 2, 3, 4, 5, Dj 
// 输出 


cout << matrix 23 << endl; 


// HB Oiz E 4E E PH OK 
for (int i20; i<1; i++) 
for (int j=0; j«2; j++) 


cout««matrix 23(i,j)««endl; 


’ 


v 3d << 9, 2, 1; 


// 矩阵 和 向 量 相 来 (X ERE 40 AB BE Fo 4E E ) 


// 但 是 在 这 里 你 不 能 混合 两 种 不 同类 型 的 矩阵 ， 像 这 样 是 错 的 


// Eigen::Matric«double, 2, 1> result wrong type = matriz 23 * v 3d; 


// 应该 显 式 转换 


Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v 3d; 


cout << result << endl; 


// 同样 你 不 能 摘 错 矩阵 的 维度 

// 斌 着 取消 下 面 的 注释 ， 看 看 会 报 什 么 错 
// Eigen: :Matriz<double, 2, 3> result wrong dimension 
»() * v 3d; 


// 一 些 和 矩阵 运算 
// 四 则 运算 就 不 演示 了 ， 直 接 用 对 应 的 运算 符 即 可 。 
matrix 33 = Eigen::Matrix3d::Random(); 
cout << matrix 33 «« endl «« endl; 


cout 
cout 
cout 
cout 
cout 


cout 


«« 
«« 
«« 
<< 
<< 
<< 


matrix_33.transpose() << endl; 
matrix_33.sum() << endl; 
matrix_33.trace() << endl; 
10*matrix_33 << endl; 
matrix_33.inverse() << endl; 


matrix_33.determinant() << endl; 


// 特征 什 
// 实 对 称 和 矩阵 可 以 保证 对 角 化 成 功 


Eigen: :SelfAdjointEigenSolver<Eigen: :Matrix3d> eigen solver ( matrix 33. 


transpose() * matrix 33 ); 


cout << "Eigen values = " << eigen solver.eigenvalues() << endl; 
cout << "Eigen vectors = " << eigen solver.eigenvectors() << endl; 
// 解 方 程 


// 我 们 求解 matriz NN * t = v Nd 这 个 方程 


// 转 置 
// 各 元 素 和 
// 迹 

// Ak X 

// i 
STATIN X 


// N fj XobdEd ray X €x, BE HMMA KH 


// 直接 求 北 自 然 是 最 直接 的 ， 但 是 求 六 运算 量 大 


Eigen::Matrix« double, MATRIX SIZE, MATRIX SIZE > matrix NN; 
matrix NN = Eigen::MatrixXd::Random( MATRIX SIZE, MATRIX SIZE ); 
Eigen::Matrix« double, MATRIX SIZE, 


1> v Nd; 


v Nd = Eigen::MatrixXd::Random( MATRIX SIZE,1 ); 


clock t time stt = clock(); // 计时 


matriz 23.cast«double 


85 // Rd 
86 Eigen::Matrix«double,MATRIX SIZE,1» x = matrix NN.inverse()*v Nd; 
87 cout <<"time use in normal invers is " << 1000* (clock() - time stt)/(double) 


CLOCKS PER SEC << "ms"<< endl; 


89 // ih HAMA ARLXGE, flde QR 分 解 ， 速 度 会 快 很 多 


90 time stt = clock(); 
91 x = matrix NN.colPivHouseholder(Qr().solve(v Nd); 
92 cout <<"time use in (r compsition is " <<1000* (clock() - time stt)/(double) 


CLOCKS, PER, SEC <<"ms" << endl; 


94 return 0; 





这 个 例 程 演示 了 Eigen 官 阵 的 基本 操作 与 运算 。 要 编译 它 ， 需 要 在 
CMakeLists.txt 里 指定 Eigen 的 头 文 件 目录 : 


1 |# 添加 头 文 件 


2 | include directories( "/usr/include/eigen3" ) 





HS —id, B7jyEigenEe KAAL, PrO A eG 
—— ease 句 将 程序 链接 到 库 上 。 不 过 ， 对 于 其 他 大 部 分 库 ， 
多 数 时 候 需 要 用 到 链接 命令 。 这 里 的 做 法 并 不 见得 是 最 好 的 ， 因 为 其 他 
人 可 能 把 Eigen 安 装 在 了 不 同位 置 ， 那 么 就 必须 手动 修改 这 里 的 涉 文 件 
目录 。 在 之 后 的 工作 中 ， 我 们 会 使 用 fi nd_package 命 令 去 搜索 库 ， 不 过 
在 本 讲 中 暂时 保持 这 个 样子 。 编 译 好 这 个 程序 后 ， 运 行 它 ， 可 以 看 到 各 
矩阵 的 输出 结果 。 


4 build/eigenMatrix 


l1 
3 |4 5 6 


0.680375 0.59688 -0.329554 
-0.211234 0.823295 0.536459 
0.566198 -0.604897 -0.444451 





AFERE PA TEMRE, ERA — REORE BRETT IS D. 
本 书 中 ， 我 们 将 仅 给 出 几 处 重要 地 方 的 说 明 《 后 面 的 实践 部 分 亦 将 体 持 
这 个 风格 ) 。 

1. 谈 者 最 好 杀手 输入 一 过 上 面 的 代码 〈 不 包括 注释 ) o RIDERE 
运行 一 遍 上 面 的 程序 。 

2.Kdevelop 可 能 不 会 提示 C++ 成 员 运 算 ， 这 和 古 它 做 得 不 够 完善 导致 
的 。 请 照 a ERA AA Bay, PVRS E ee AEA EA TR 

3.Eigente FEN FE MEFIMATLAB{R AHI, JLF A HY Bie B SE AB 
阵 来 处 理 。 但 是 ， 为 了 实现 更 好 的 效率 ， 在 Eigen 中 需要 指定 矩阵 的 大 
小 和 类 型 。 对 于 在 编译 时 期 束 知 道 大 小 的 矩阵 ， 处 理 起 来 会 比 动态 变化 
大 小 的 矩阵 更 快 一 些 。 因 此 ， 像 旋转 和 矩阵 、 变 换 趣 阵 这 样 的 数据 ， 完 全 
可 在 编译 时 期 确定 它们 的 大 小 和 数据 类 型 。 

4.Eigen 内 部 的 矩阵 实现 比较 复杂 ， 这 里 不 做 介绍 ， 我 们 和 希望 你 像 使 
用 fl oat、double 等 内 阐 数 据 类 型 那样 使 用 Eigen 的 窍 阵 。 这 应 该 是 人 符合 
其 设计 初衷 的 。 

5.Eigen 知 阵 不 支持 自动 类 型 提升 ， 这 和 C++ 的 内 建 数据 类 型 有 较 大 
磊 寞 。 在 C++ 程 序 中 ， 我 们 可 以 把 一 个 fl oat 数 据 和 double 数 据 相 加 、 相 
乘 ， 编 译 剖 会 日 动 把 数据 类 型 转换 为 最 合适 的 那 种 。 而 在 Eigen 中 ， 出 
于 性 能 的 考虑 ， 必 须 显 式 地 ”对 答 隆 类 型 进行 转换 。 而 如 果 起 了 这 样 
做 ，Eigen 会 (不 太 友 好 地 ) 提示 你 一 个 “YOU MIXED DIFFERENT 
NUMERIC TYPES...” 的 编译 错误 。 你 可 以 笑 试 找 一 下 这 条 信息 出 现在 错 


误 提 示 的 哪个 部 分 。 如 果 错 误 信 息 太 长 最 好 保存 到 一 个 文件 里 再 找 。 

6. 同 理 ， 在 计算 过 程 中 也 需要 保证 矩阵 维 数 的 正确 性 ， 否 则 会 出 
现 “YOU MIXED MATRICES OF DIFFERENT SIZES” 错 误 。 请 不 要 抱怨 
这 种 错误 提示 方式 ， 对 于 C++ 模板 元 编程 ， 能 够 提示 出 可 以 阅读 的 信息 
已 经 是 很 幸运 的 了 。 以 后 ， 若 发 现 Eigen 出 错 ， 你 可 以 直接 寻找 大 写 的 
部 分 ， 推 测 出 了 什么 问题 。 

7. 我 们 的 例 程 只 介绍 了 基本 的 窍 阵 运 算 。 你 可 以 疯 旋 
http://eigen.tuxfamily.org/dox-devel/modules.html^£ J # 4 HJ Eigen 4 i . 
eR yeas S mee AE op, HEC TN EE NSE LAE i PAE 
作 Eigen 。 

最 后 一 段 代 人 码 中 比较 了 求 逆 与 求 QR 分 解 的 运行 效率 ， 你 可 以 看 看 
Hos EWN AH, Peake aA HA 55 HJ 28 0E? 


3.3 We FS [a] ee ARK Fir H 
3.3.1 JEFE 


我 们 重新 回 到 理论 部 分 。 有 了 旋转 窍 阵 来 摘 述 旋转 ， 有 了 变换 窍 阵 
拍 述 一 个 6 目 由 度 的 三 维 刚体 运动 ， 是 不 是 已 经 足够 了 呢 ? 窍 阵 表 示 方 
TLD ALR LAMA: 


1.SO(3) 的 旋转 矩阵 有 9 个 量 ， 但 一 次 旋转 只 有 3 个 自由 度 。 因 此 这 种 
表达 方式 是 见 余 的 。 同 理 ， 变 换算 阵 用 16 个 量 表达 了 6 上 日 由 度 的 变换 。 
WA, AUSSEN? 


2. 旋 转 和 矩阵 自身 带 有 约束 : 它 必须 是 个 正 交 矩阵， 且 行 列 式 为 1。 
变换 窍 阵 也 是 如 此 。 当 想 要 估计 或 优化 一 个 旋转 矩阵 /变换 算 阵 时 ， 这 
些 约束 会 使 得 求解 变 得 更 困难 。 

因此 ， 我 们 希望 有 一 种 方式 能 够 紧凑 地 摘 述 旋转 和 平移 。 例 如 ， 用 
一 个 三 维 癌 量 表达 旋转 ， 用 六 维 癌 量 表达 变换 ， 可 行 吗 ? 事实 上 ， 在 前 
面 介绍 外 积 的 那 部 分 ， 我 们 提 到 过 如 何 做 这 件 事 。 我 们 介绍 了 如 何 用 外 
只 表达 两 个 同 量 的 旋转 关系 。 对 于 坐标 系 的 旋转 ， 我 们 知道 ， 任 意 旋转 
都 可 以 用 一 个 旋转 轴 和 一 个 旋转 角 ”来 刻画 。 于 是 ， 我 们 可 以 使 用 一 个 
回 量 ， 其 方 回 与 旋转 轴 一 致 ， 而 长 度 等 于 旋转 角 。 这 种 回 量 称 为 旋转 回 
m CH. Axis-Angle) 。 这 种 表示 法 只 需 一 个 三 维 回 量 即 可 摘 述 旋 
转 。 同 样 ， 对 于 变换 窍 阵 ， 我 们 使 用 一 个 旋转 同 量 和 一 个 平移 同 量 即 可 
表达 一 次 变换 。 这 时 的 维 数 正 好 是 六 维 。 

事实 上 ， 旋 转 同 量 束 是 下 一 讲 准 备 介绍 的 李 代 数 。 所 以 本 讲 中 读者 
只 需 知 道 旋 转 可 以 这 样 表示 即 可 。 剩 下 的 问题 是 ， 旋 转 同 量 和 旋转 窍 阵 
之 间 是 如 何 转换 的 呢 ? 假设 有 一 个 旋转 轴 为 n ， 和 角度 为 8 NERS, w 
然 ， 它 对 应 的 旋转 同 量 为 I9n ”。 从 旋转 回 量 到 谍 转 矩阵 的 转换 过 程 由 罗 
德里 格 斯 公式 CRodrigues's Formula) 表明 ， 由 于 推导 过 程 比较 复杂 ， 
这 里 不 作 描 述 ， 只 给 出 转换 的 结果 里 : 


R = cos0I + (1 — cos0) nn! + sin n^. (3.14) 


从 写 个 是 问 量 到 反对 称 的 转换 符 ， 见 式 (3.30 。 反 之 ， 我 们 也 可 
以 计算 从 一 个 旋转 滤 阵 到 旋转 问 量 的 转换 。 对 于 转角 9 ， 有 : 





tr(R) = cosôtr(I)+ (1-— cos0)tr (nn) + sinOtr(n’) 
= 3cos0O 二 (1 一 cosO0) (3.15) 
= l-42cosÓ. 

因此 : 
0 一 arccos TY = ). (3.16) 


天 于 转轴 mn ， 由 于 旋转 轴 上 的 同 量 在 旋转 后 不 友 生 改变 ， 说 明 
Rn = n. 


因此 ， 转 轴 n 7270 MER 特征 值 1 对 应 的 特征 回 量 。 求 解 此 方程 ， 再 归 
一 化 ， 束 得 到 了 旋转 轴 。 读 者 也 可 以 从 * 旋 转轴 经 过 旋转 之 后 不 变 ” 的 几 
何 角度 看 待 这 个 方程 。 顺 便 提 一 下 ， 这 里 的 两 个 转换 公式 在 下 一 讲 仍 将 
出 现 ， 你 会 发 现 它们 正 是 SO(3) 上 李 群 与 李 代 数 的 对 应 关系 。 


3.3.2 KRAM 


下 面 来 说 说 欧 拉 角 。 

无 论 是 旋转 矩阵 、 旋 转 同 量 ， 它 们 虽然 能 描述 旋转 ， 但 对 我 们 人 类 
是 非常 不 直观 的 。 当 我 们 看 到 一 个 旋转 矩阵 或 旋转 同 量 时 ， 很 难 想象 出 
这 个 旋转 究 葛 是 什么 样 的 。 当 它们 变换 时 ， 我 们 也 不 知道 物体 是 同 哪 个 
方 同 在 转动 。 而 欧 拉 角 则 提供 了 一 种 非常 直观 的 方式 来 摘 述 旋转 一 C 
使 用 了 3 个 分 离 的 转角 ， 把 一 个 旋转 分 解 成 3 次 绕 不 同 轴 的 旋转 。 当 
然 ， 由 于 分 解 方式 有 许多 种 ， 所 以 欧 拉 角 也 存在 着 不 同 的 定义 方法 。 比 
如 说 ， 先 经 X üt. HSSY 轴 ， 最 后 绕 Z 轴 ， 束 得 到 了 一 个 XY Z BUT 
旋转 。 同 理 ， 可 以 定义 ZY Z 、ZY X 等 旋转 方式 。 如 果 讨 论 得 更 细 一 
些 ， 还 需要 区 分 每 次 是 绕 固 定 轴 ”旋转 的 ， 还 是 绕 旋 转 之 后 的 轴 旋转 
的 ， 这 也 会 给 出 不 一 样 的 定义 方式 。 





你 或 许 在 航空 、 舱 模 中 听 说 过 “俯仰 攻关 偶 航 角 ” 这 些 词 。 欧 拉 角 当 
中 比较 第 用 的 一 种 ， 便 是 用 “ 偏 豚 -信仰 - 深 转 ”(yaw-pitch-roll〉3 个 角 展 
来 描述 一 个 旋转 的 。 由 于 它 等 价 于 ZY x 轴 的 旋转 ， 因 此 就 以 ZY X 为 
例 。 假 设 一 个 刚体 的 前 方 〈 瑚 回 我 们 的 方 问 ) 为 X 轴 ， 右 侧 为 Y $n, E 
方 为 Z SH, ünEd3a-3Bpgs. HA, ZY X 转角 相当 于 把 任意 旋转 分 解 成 以 
下 3 个 轴 上 的 转角 : 


1. 纸 物体 的 Z Fe, FB at A yaw: 
2. 统 旋转 之 后 的 了 轴 旋 转 ， 得 到 从 仰角 pitch; 
3.5 Peor Ja 的 X 轴 讶 转 ， 得 到 深 转 角 roll。 


此 时 ， 可 以 使 用 [r,p,y ] 这 样 一 个 三 维 的 癌 量 描述 任意 旋转 。 这 个 
品 量 十 分 直观 ， 我 们 可 以 从 这 个 疝 量 想 象 出 旋转 的 过 程 。 其 他 的 网 拉 角 
亦 是 通过 这 种 方式 ， 把 旋转 分 解 到 3 个 轴 上 ， 得 到 一 个 三 维 的 向 量 ， 只 
不 过 选用 的 轴 及 顺序 不 一 样 。 这 里 介绍 的 rpy 角 是 比较 常用 的 一 种 ， 只 
有 很 少 的 欧 拉 角 种 类 会 有 rpy 这 样 脸 旬 人 口 的 名 字 。 不 同 的 欧 拉 角 是 按 
照 旋 转轴 的 顺序 来 称呼 的 。 例 如 ，rpy 角 的 旋转 顺序 是 ZY X 。 同 样 ， 也 
有 XY ZZY Z 这 样 的 欧 拉 角 但 是 它们 就 没有 专门 的 名 字 了 。 值 得 一 
提 的 是 ， 大 部 分 领域 在 使 用 欧 拉 角 时 都 有 各 目的 坐标 方 回 和 顺序 上 的 习 
惯 ， 不 一 定 和 我 们 这 里 说 的 相同 。 

吹 拉 角 的 一 个 重大 缺点 是 会 磁 到 著名 的 万 同 锁 问题 (Gimbal Lock"! 
) : 在 俯仰 角 为 + 90 ^ 时， 第 一 次 旋转 与 第 三 次 旋转 将 使 用 同一 个 轴 ， 
使 得 系统 丢失 了 一 个 自由 度 〈( 由 3 次 旋转 变 成 了 2 次 旋转 ) 。 这 被 称 为 奇 
异性 问题 ， 在 其 他 形式 的 欧 拉 角 中 也 同样 存在 。 理 论 上 可 以 证 明 ， 只 要 
想 用 3 个 实数 来 表达 三 维 旋转 时 ， 都 会 不 可 避免 地 合 到 奇异 性 问题 。 由 
于 这 种 原理 ， 欧 拉 角 不 适 于 插值 和 友 代 ， 往 往 只 用 于 人 机 交互 中 。 我 们 
也 很 少 在 SLAM 程 序 中 直接 使 用 欧 拉 角 表达 姿态 ， 同 样 不 会 在 滤波 或 优 
化 中 使 用 欧 拉 角 表达 旋转 〈 因 为 它 具 有 奇异 性 ) 。 不 过 ， 若 你 想 验 证 上 自 
己 的 算法 是 否 有 错 ， 转 换 成 欧 拉 角 能 够 快速 分 辨 结果 是 否 正 确 。 








原始 坐标 系 第 三 次 旋转 
万 问 锁 的 情况 


第 一 次 旋转 第 二 次 旋转 为 90 度 第 三 次 旋转 变 成 和 第 一 次 相同 
图 3-3” 欧 拉 角 的 旋转 示意 图 。 上 方 为 ZYX 角 定义 。 下 方 为 pitch=90“ 时， 第 三 次 旋转 与 第 
一 次 深 转 角 相 同 ， 使 得 系统 丢失 了 一 个 自由 度 。 如 果 你 还 没有 理解 万 同 锁 ， 可 以 看 看 相 
天 视频 ， 理 解 起 来 会 更 方便 。 


3.4 ”四 元 数 
3.4.1 四 元 数 的 定义 


旋转 窍 阵 用 9 个 量 描述 3 目 由 上 度 的 旋转 ， 基 有 元 余 性 ; 欧 拉 角 和 旋转 
回 量 是 紧凑 的 ， 但 具有 奇异 性 。 事 实 上， 我 们 找 不 到 不 带 奇 异性 的 三 维 
器 量 描述 方式 O 。 这 有 点 类 似 于 用 两 个 坐标 表示 地 球 表 面 〈 如 经 上 度 和 
纬度 ) ， 将 必 完 存在 奇异 性 《纬度 为 + 90° WARM) 。 三 维 旋转 
是 一 个 三 维 流 形 ， 想 要 无 奇异 性 地 表达 它 ， 用 3 个 量 是 不 够 的 。 

回忆 以 前 学 习 过 的 复数 。 我 们 用 复数 集 C 表 示 复 平面 上 的 同 量 ， 而 
复数 的 乘法 则 表示 复 平 面 上 的 旋转 : 例如 ， 乘 上 复数 | 相当 于 送 时 针 把 
一 个 复 同 量 旋转 90 ”。 类 似 地 ， 在 表达 三 维 空间 旋转 时 ， 也 有 一 种 类 似 
于 复数 的 代数 : 四 元 数 CQuaternion) 。 四 元 数 是 Hamilton 找 到 的 一 种 
扩展 的 复数 。 它 既是 么 读 的 ， 也 没有 奇异 性 。 如 有 果 说 缺点 ， 四 元 数 不 
Ue ED. Rus SIE, 

一 个 四 元 数 q 拥有 一 个 实 部 和 三 个 虚 部 。 本 书 把 实 部 写 在 前 面 (也 
有 地 方 把 实 部 与 在 后 面 )， 像 下 和 面 这 样 : 


q = qo t qii 4 +t gak, (3.17) 


其 中 ij,k 为 四 元 数 的 三 个 虚 部 。 这 三 个 虚 部 满足 以 下 关系 式 : 


i. et E 

id dh dic È 

dili (3.18) 
jk =i,kj — —i 


ki = j ik= -j 


由 于 它 的 这 种 特殊 表示 形式 ， 有 时 人 们 也 用 一 个 标量 和 一 个 问 量 来 
表达 四 元 数 : 


q = |s, v], s — qo ER, v = (11,92, 33]! € R, 


XE, s 称 为 四 元 数 的 实 部 ， 而 w 称 为 它 的 虚 部 。 如 果 一 个 四 元 数 
的 虚 部 为 0， 称 之 为 实 四 元 数 。 反 之 ， 知 它 的 实 部 为 0， 则 称 之 为 虚 四 
TER 。 

这 和 复数 非常 相似 。 考 虑 到 三 维 空间 需要 3 个 轴 ， 四 元 数 也 有 3 个 虚 
部 ， 那 么 ， 一 个 虚 四 元 数 能 不 能 对 应 到 一 个 空间 点 呢 ? 事 实 上 我 们 整 是 
这 样 做 的 。 同 理 ， 我 们 知道 一 个 模 长 为 1 的 复数 可 以 表示 复 平 面 上 的 纯 
旋转 〈 没 有 长 度 的 缩放 ) ， 那 么 ， 三 维 空 间 中 的 旋转 是 否 能 用 单位 四 元 
数 表达 呢 ? 答案 也 是 肯定 的 。 

我 们 能 用 单位 四 元 数 “” 表示 三 维 空间 中 任意 一 个 旋转 ， 不 过 这 种 表 
达 方 式 和 复数 有 关 微 妙 的 不 同 。 在 复数 中 ， 乘 以 i 意味 着 旋转 90 ”。 这 
是 否 意味 着 四 元 数 中 ， 乘 i EAE SR 轴 旋 转 90“? 那么 ,六 =-k SE 
di. FESR 90°, Hidéj £90, WASP Sek 5-90"? 读者 可 以 找 一 部 
手机 比划 一 下 一 一 然后 你 会 发 现 情况 并 不 是 这 样 。 正 确 的 情形 应 该 是 ， 
HELI 对 应 着 旋转 180”， 这 样 才 能 保证 六 =-k 的 性 质 。 而 i ? =-1, RIRA 
ei 轴 旋 转 360“ 后 得 到 一 个 相反 的 东西 。 这 个 东西 要 旋转 两 周 才 会 和 它 
原先 的 样子 相等 。 

这 似乎 有 些 玄妙 了 ， 完 整 的 解释 需要 引入 太 多 额外 的 东西 ， 我 们 还 
是 冷静 一 下 回 到 眼前 。 至 少 ， 我 们 知道 单位 四 元 数 能 够 表达 三 维 空间 的 
旋转 。 这 种 表达 方式 和 旋转 矩阵 、 旋 转 癌 量 有 什么 关系 昵 ? 我 们 不 妨 先 
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0 的 旋转 ， 那 么 这 个 旋转 的 四 元 数 形式 为 





6 0 09 0 
q = |cos z’ na sin zy sin z’ Me sin 了 i (3.19) 


RZ, JEA Muro T h AT E Fe SSK A: 


= 2 arccos qo (3.20) 
LN ny,n;]. = |q1, G2, qs] /sin £ 


这 个 式 子 给 了 我 们 一 种 微妙 的 “ 转 了 一 半 ” 的 感光 。 同 样 ， 对 式 
(3.19) 的 0 Jn E2m ， 我 们 得 到 一 个 相同 的 旋转 ， 但 此 时 对 应 的 四 元 数 
变 成 了 -q ” 。 因 此 ， 在 四 元 数 中 ,任意 的 旋转 都 可 以 由 两 个 互 为 相反 数 
的 四 元 数 表 示 。，。 同 理 ， 取 9 ”为 0， 则 得 到 一 个 没有 任何 旋转 的 实 四 元 
数 : 


qo = [ 土 1, 0, 0,0] . (3.21) 
— DA +— er 
3.4.2 ”四 元 数 的 运算 
四 元 数 和 通 钊 复数 一 样 ， 可 以 进行 一 系列 的 运算 。 间 见 的 有 四 则 运 
AL. ZG. Sew, FEMUR. OR PITA. 
现 有 两 个 四 元 数 qg .q,> VI Sea Als, ov, l, is, ,v, ]， 或 者 原 
始 四 元 数 表示 为 
da = Sa d- Zai- yaj + Zak, Go = Sp + Lot + yj + Zk. 
那么 ， 其 运算 可 表示 如 下 。 
1. 加 法 和 减法 
四 元 数 q_ qu 的 加 减 运 算 为 
Ga + qy = [Sa E 50, Va £ Vp]. (3.22) 
2. 乘 法 
乘法 是 把 gqg， 的 每 一 项 与 qg， 的 每 项 相 乘 ， 最 后 相 加 ， 虚 部 要 按照 式 
(3.18) 进行 。 整 理 可 得 : 
dadb = SaSb— XaXb — YaYb — %aX%bd 
+ (Sah T X335 T Yab 一 Zato) 1 (3.23) 
+ (Sab — Laz + YaSs + Zatb) j 
+ (SaZb + tab — Yap + ZaSp) k. 


虽然 条 为 复式 ， 但 形式 上 是 整齐 有 序 的 。 如 条 写 成 网 量 形 陈 并 利用 
内 外 积 运算 ， 该 表达 会 更 加 催 洁 : 


Qadi = |sasb — vl Vp, SaVp + SyUg + Va X Vp | i (3.24) 


在 该 乘法 定义 下 ， 两 个 实 的 四 元 数 乘积 仍 古 实 的 ， 这 与 复数 也 是 一 
AH). JAM, TERS), Aaa OUR EE, DUC Ree E eS 
可 交换 的 ， 除 非 v, 和 v, YER? 中 共 线 ， 此 时 外 积 项 为 零 。 

3.HE 

PU ToC A SE Xe HE E SE EDU AB c 230 : 


i658. 4 = 9.9 — BES [Bae Ua (3.25) 


URSA SADR, SBI PTR, RD BI 


qq-—qq' = [s? 十 viv, 0]. (3.26) 
4. 模 长 
四 元 数 的 模 长 定义 为 

dal] = V sz + xå va + zá- (3.27) 


可 以 验证 ， 两 个 四 元 数 乘 积 的 模 即 为 模 的 乘积 。 这 保证 了 单位 四 元 
数 相 乘 后 仍 是 里 位 四 元 数 。 


aaaol = llgqallllgoll. (3.28) 
5. ijf 
—^r JU zo CES 3 7 

q —4'/|all. (3.29) 


按 此 定义 ， 四 元 数 和 自己 的 逆 的 乘积 为 实 四 元 数 1: 


qq =q q=1. (3.30) 


如 果 q Abu, RAMRAM EAE. TRIP, FAA 
A AA EE EL ALAS TE JR : 


(qaq) = qs qa (3.31) 
6.253 I6 5 AF 
和 问 量 相似 ， 四 元 数 可 以 与 数 相 乘 : 
kq = [ks, kv]. (3.32) 
点 滋 是 指 两 个 四 元 数 每 个 位 置 上 的 数值 分 别 相 乘 : 
Qa ` qb = SaSb + Labi + Yayo) + Zazok. (3.33) 


3.4.3 ”用 四 元 数 表 示 旋 转 


我 们 可 以 用 四 元 数 表达 对 一 个 点 的 旋转 。 假 设 一 个 空间 三 维 点 p = 
[x,y,z ]E Ra ， 以 及 一 个 由 轴 角 n,0 指定 的 旋转 。 三 维 点 p 经 过 旋转 之 后 
变 为 p 。 如 果 使 用 矩阵 接 述 ， 那 么 有 p”=Rp 。 而 如 果 用 四 元 数 描述 旋 
转 ， 它 们 的 关系 又 如 何 来 表达 呢 ? 


首 匈 ， 把 三 维 空间 点 用 一 个 虚 四 元 数 来 拍 述 : 
p = [0,2,y, 2] = [0, v]. 


这 相当 于 把 四 元 数 的 3 个 虚 部 与 空间 中 的 3 个 轴 相 对 应 。 然 后 ， 参 照 
式 (3.19)， 用 四 元 数 g 表示 这 个 放 转 : 


em 7] 
= (COS — SITI hg 
4 2 2 


HA, MEFE Ap 即 可 表示 为 这 样 的 乘积 : 


p —qpq +. (3.34) 


可 以 验证 ( 留 作 习题 )， 计 算 结 来 的 实 部 为 0， 故 为 纯 虚 四 元 数 。 
其 虚 部 的 3 个 分 量 表示 旋转 后 3D 扣 的 坐标 。 


3.4.4 VU rc By Fl) DE Fe HE HI FE FR 


任意 里 位 四 元 数 括 述 了 一 个 旋转 ， 访 旋转 汪 可 用 旋转 矩阵 或 旋转 问 
量 插 述 。 从 旗 转 同 量 到 四 元 数 的 转换 方式 已 在 式 (3.200 中 给 出 。 因 
此 ， 现 在 看 来 把 四 元 数 转 换 为 矩阵 的 最 且 观 方法 ， 古 先 把 四 元 数 q 转换 
AUSF Mn ， 然 后 再 根据 岁 德里 格 斯 公式 转换 为 窍 阵 。 不 过 那样 要 计 
算 一 个 arccos 图 数 ， 代 价 较 大 。 实 际 上 这 个 计算 是 可 以 通过 一 定 的 技巧 
纪 过 的 。 这 里 省 略 推导 过 程 ， 直 接 给 出 四 元 数 到 旋转 窍 阵 的 转换 方式 。 


设 四 元 数 q =q , +9q ii+qg,j +qg KK， 对 应 的 旋转 矩阵 R 为 


1— 245 —2q3 2qıq2 + 2qoq3  2q1q3 — 240q2 
R= | 249142 — 2qoq3 1—2q1 —2q3 2q293 + 240i (3.35) 


29193 + 2qoq2 2q2q3 — 2qoqı 1 — 2q% — 245. 


反之， 由 旋转 矩阵 a 到 四 元 数 的 转换 如 下 。 假 设 窍 阵 为 R ={m, hijs 
[1, 2, 3]， 其 对 应 的 四 元 数 g 由 下 式 给 出 : 


tr(R)+1 —  maa3— mag mam mi2— moi 
— Å— — 349 Á Tr ———— 
4q0 4q0 4q0 


值得 一 所 的 是 ， 由 于 q 和 -q xml] ee, SEE —TSR 对 应 的 
四 元 数 表示 并 不 是 唯一 的 。 同 时 ， 除 了 了 上面 给 出 的 转换 方式 之 外 ， 还 行 
在 其 他 几 种 计算 方法 ， 而 本 书 都 省 略 了 。 实 际 编程 中 ， 当 q o 接近 0 时 ， 
其 余 3 个 分 量 会 非常 大 ， 了 叶 色 解 不 稳定 ， 此 时 我 们 再 考虑 使 用 其 他 的 方 
AGE TR. 

最 后 ， 无 论 征 四 元 数 、 旋 园 矩 阵 还 是 轴 角 ， 它 们 都 可 以 用 来 持 述 同 
一 个 旋转 。 我 们 应 该 在 实际 中 选择 最 为 方便 的 形式 ， 而 不 必 拘 泥 于 采种 
特定 的 形式 。 在 随后 的 实践 和 习题 中 ， 我 们 会 泗 示 各 种 表达 方式 之 间 的 


(3.36) 





转换 ， 以 加 深 读 者 的 印象 。 


3.5 "HT. Dij. HINC AETA 


3D 空 间 中 的 变换 ， 除 了 欧 氏 变换 之 外 ， 还 存在 其 他 几 种 ， 只 不 过 
欧 氏 变换 是 最 简单 的 。 它 们 一 部 分 和 测量 几何 有 关 ， 因 为 在 之 后 的 讲解 
中 可 能 会 提 到 ， 所 以 完 罗列 出 来 。 欧 氏 变 换 保持 了 问 量 的 长 有 度 和 来 和 角 ， 
相当 于 我 们 把 一 个 刚体 原封 不 动 地 进行 了 移动 或 旋转 ， 不 改变 它 目 里 的 
样子 。 而 其 他 几 种 变换 则 会 改变 它 的 外 形 。 它 们 部 拥有 类 似 的 矩阵 表 
不 。 


1. 相 似 变 换 


相似 变换 比 欧 氏 变 换 多 了 一 个 目 由 度 ， 它 允许 物体 进行 均匀 缠 放 ， 
ROBES AY 
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TE Ih PUR Ba E SSA Ts  ， 表 示 我 们 在 对 问 量 旋转 之 
后 ， 可 以 在 xy,z 三 个 坐标 上 进行 均匀 缩放 。 由 于 含有 缩放 ， 相 似 变 换 不 
再 你 持 图 形 的 面积 不 变 。 你 可 以 想象 一 个 边 长 为 1 的 立方 体 通 过 相似 变 
换 后 ， 变 成 边 长 为 10 的 样子 (但 仍然 是 立方 体 )。 

2.05 5] SER. 

ITZE PREY EBA ECAN F: 


A t 
0L 1 


T= | (3.38) 








与 欧 氏 变换 不 同 的 是 ， 仿 射 变 换 只 要 求 A 是 一 个 可 逆 窍 阵 ， 而 不 必 
是 正 交 和 矩阵 。 仿 射 变换 也 叫 正 交 投 影 。 经 过 仿 射 变换 之 后 ， 立 方 体 就 不 
再 是 方 的 了 ， 但 是 各 个 面 仍然 是 平行 四 边 形 。 

3. 册 影 变 换 


DARPA re Ee CHR, EKER IB CA 


(3.39) 








tH EAA BEE REA ， 右 上 和 角 为 平移 t A r AKIMA. H 
FKH IREE, Hv =0 时 ， 我 们 可 以 对 整个 矩阵 除 以 v 得 到 一 个 右 
下 角 为 1 的 和 矩阵， 盏 则 得 到 右 下 和 角 为 0 的 矩阵 。 因 此，2D 的 射影 变换 一 
共有 8 个 自由 上 度 ，3D 则 共有 15 个 上 自由 上 度 。 射 影 变 换 是 现在 讲 过 的 变换 
中 ， 形 式 最 为 一 般 的 。 从 真实 世界 到 相机 照片 的 变换 可 以 看 成 一 个 射影 
变换 。 读 者 可 以 想象 一 个 原本 方形 的 地 板 砖 ， 在 照片 当中 是 什么 样子 : 
首先 ， 它 不 再 是 方形 的 。 由 于 近 大 远 小 的 天 系 ， 它 甚至 不 是 平行 四 边 
形 ， 而 是 一 个 不 规则 的 四 边 形 。 


表 3-1 总 结 了 目前 讲 到 的 几 种 变换 的 性 质 。 注 意 在 “不 变性 质 "中 ， 
从 上 到 下 是 有 包含 关系 的 。 例 如 ， 欧 氏 变换 除了 保 体积 之 外 ， 也 具有 保 
平行 、 相 交 等 性 质 ， 


表 3-1 第 见 变 换 性 质 比 较 


变换 名 称 | 和 矩阵 形式 ` 变 性质 
6 KBE. Kf. PSR 
7 体积 比 
12 平行 性 、 体 积 比 


15 接触 平面 的 相交 和 相 切 





我 们 之 后 会 说 到 ， 从 真实 世界 到 相机 照 上 户 的 变换 是 一 个 射影 变换 。 
如 末 相 机 的 焦距 为 无 穷 远 ， 那 么 这 个 变换 为 仿 射 变换 。 不 过 ， 在 详细 讲 
述 相机 模型 之 前 ， 我 们 只 要 对 它们 有 个 大 致 的 印象 即 可 。 


3.6 ”实践 : Eigen 几 何 模 块 


现在 ， 我 们 来 实际 汗 练 一 下 前 面 讲 到 的 各 种 旋转 表达 方式 。 我 们 将 
在 Eigen 中 使 用 四 元 数 、 欧 拉 角 和 旋转 窍 阵 ， 诗 示 它 们 之 间 的 变换 方 
去。 我 们 还 会 给 出 一 个 可 视 化 程序 ， 帮 助 读 者 理解 这 几 个 变换 的 天 系 。 


中 slambook/ch3/useGeometry/useGeometry.cpp 


#include <iostream> 


#include <cmath> 


using namespace std; 


#include <Eigen/Core> 
// Eigen 几何 模块 


#include <Eigen/Geometry> 


DOO IO oe ok IOI II IO 
* 本 程序 演示 了 Eigen 几何 模块 的 使 用 方法 


aj o jeje oe oe oje je oe je oe je be je oe ak kk oe be oe kk ak ak ake teak / 


int main( int argc, char** argv ) 


1 


// Eigen/Geometry 模块 提供 了 各 种 旋转 和 平移 的 表示 

// 3D 旋转 矩阵 直接 使 用 Matriz3d 或 Matriz3f 

Eigen::Matrix3d rotation matrix = Eigen::Matrix3d::Identity(); 

// 旋转 向 量 使 用 4ngle4zis， 它 底层 不 直接 是 Matris , WERT YA d EAE ( 因为 重 载 
了 运算 符 ) 

Eigen::AngleAxisd rotation vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); // 
i Z 轴 旋 转 45 Š 

cout .precision(3); 

cout<<"rotation matrix =\n"<<rotation_vector.matrix() ««endl; Z 
用 matriz() 转换 成 矩阵 

// 也 可 以 直接 赋值 

rotation matrix = rotation vector.toRotationMatrix(); 

// 用 AngleAzis 可 以 进行 坐标 变换 

Eigen::Vector3d v ( 1,0,0 ); 

Eigen::Vector3d v rotated - rotation vector * v; 

cout<<"(1,0,0) after rotation = "««v rotated.transpose()««endl; 

// 或 者 用 旋转 和 矩阵 

v rotated = rotation matrix * v; 


cout<<"(1,0,0) after rotation = "««v rotated.transpose()««endl; 


// Bk4 ji TVA AS ae Fe AE E AA 4G A X EC f 
Eigen::Vector3d euler angles = rotation matrix.eulerAngles ( 2,1,0 ); // ZYX 顺 
Fe, BP yaw pitch roll 顺序 


cout<<"yaw pitch roll = "««euler angles.transpose()««endl; 


// Wk K X GR HERR A Eigen: :Isometry 


Eigen: :Isometry3d T=Eigen: :Isometry3d::Identity() ; // 虽然 称 为 3d ， 实 质 上 
是 4x4 845 Me 
T.rotate ( rotation vector ); // 按照 rotation vector 进行 旋转 


T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把 平移 向 量 设 成 (1,3,4) 


40 cout << "Transform matrix = Mn" << T.matrix() ««endl; 


2 // F RHE ERAT E HR RR 


43 Eigen::Vector3d v_transformed = T*v; // 相当 于 Revtt 

44 cout<<"v tranformed = "<<v_transformed.transpose()<<endl; 

45 

46 // 对 于 仿 射 和 射影 变换 ， 使 用 Eigen::Affine3d 和 Eigen::Projective3d PPT, % 

47 

48 // 四 元 数 

49 // 可 以 直接 把 AngleAmis 赋值 给 四 元 数 ， 反 之 亦 然 

50 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation vector ); 

51 cout<<"quaternion = \n"<<q.coeffs() ««endl; // 注意 coeffs 的 顺序 是 (2,Yy,%,w) 
w 为 实 部 ， 前 三 者 为 虚 部 

52 // 也 可 以 把 旋转 矩阵 赋 给 它 

53 q = Eigen::Quaterniond ( rotation matrix ); 

54 cout<<"quaternion = \n"<<q.coeffs() ««endl; 

55 // 使 用 四 元 数 旋 转 一 个 向 量 ， 使 用 重 载 的 乘法 即 可 

56 v rotated = q*v; // 注意 数学 上 是 quq'(-1) 

57 cout<<"(1,0,0) after rotation = "««v rotated.transpose()««endl; 

58 

59 return 0; 

é |J 


Figen 中 对 各 种 形式 的 表达 方式 总 结 如 下 。 请 注意 每 种 类 型 都 有 单 
精度 和 双 狂 度 两 种 数据 闫 型 ， 而 且 和 之 前 一 样 ， 不 能 由 编译 左 目 动 转 
换 。 下 面 以 双 精 度 为 例 ， 你 可 以 把 最 后 的 d 改 成 f， 即 得 到 单 精 度 的 数据 
结构 。 


eee Fe Be (3x 3) : Eigen::Matrix3d. 

* 诈 转 同 量 (3x1) : Eigen::AngleAxisd. 

“ 欢 拉 角 (3x 1) : Eigen: Vector3d. 

四 元 数 (4x 1) : Eigen::Quaterniond. 

X IAESRABEE (Ax 4) : Eigen::Isometry3d. 
{ITER (4x 4) : Eigen::A ffi ne3d。 


。 刚 影 变 换 (4x 4) : Eigen::Projective3d。 


我 们 把 如 何 编 详 此 程序 的 问题 交 给 谈 着 。 在 这 个 程序 中 ， 污 示 了 如 
何 使 用 Eigen 中 的 旋转 矩阵 、 旋 转 回 量 (AngleAxis) 、 欧 拉 角 和 四 元 
数 。 我 们 用 这 几 种 旋转 方式 去 旋转 一 个 同 量 y ， 友 现 结果 是 一 样 的 (不 
EMREIS) 。 同 时 ， 也 演示 了 如 何在 程序 中 转换 这 几 种 表达 方 
式 。 想 进一步 了 解 Eigen 的 几何 模块 的 读者 可 以 参考 
( http://eigen.tuxfamily.org/dox/group — TutorialGeometry.html) . 


3.7. 可视化 演示 


最 后 ， 我 们 为 读者 准备 了 一 个 小 程序 ， 位 于 
slambook/ch3/visualizeGeometry 中 。 它 以 可 视 化 的 形式 演示 了 各 种 表达 
方式 的 异同 〈( 见 图 3-4) 。 旋 者 可 以 用 鼠标 操作 一 下 ， 看 看 数据 是 如 何 
变化 的 。 


-[0.71,-0.41,0.58] , [(0.00,0.82,0.58], [-0.71,-0.41,0.5 


[-0.279848,0.364705,0.115917,0.8 76 





图 3-4 eee FEM. Wea. UTCAI EI AIE o 


在 这 个 小 程序 中 ， 我 们 在 坐标 原点 放置 了 一 个 彩色 立方 体 。 用 鼠标 
可 以 平移 /旋转 相机 。 你 可 以 实时 地 看 到 相机 姿态 的 变化 。 我 们 显示 了 
rp piu Ni. 欧 拉 角 和 四 元 数 的 3 种 姿态 ， 你 可 以 实际 体验 一 下 这 几 个 

是 如 何 变 化 的 。 然 而 根据 我 们 的 经 验 ， 除 了 欧 拉 角 之 外 ， 你 应 该 看 不 
出 它们 直观 的 含义 。 


天 于 访 程 序 的 源 代 码 ， 这 里 束 不 做 解释 了 了， 如 条 你 感 兴 趣 ， 可 以 目 
查看 。 rt bed Om ta Main 

值得 一 提 的 是 ， 实 际 中 ， 我 们 会 至 少 定 义 两 个 坐标 系 : 世界 坐标 系 

和 相机 坐标 系 。 在 该 定义 下 ， 设 某 个 点 在 世界 坐标 系 中 的 坐标 为 p,，， 


在 相机 坐标 系 下 为 p. ， 那 么 : 
Pc = Twpw,) (3.40) 


ZET, 表示 世界 坐标 系 到 相机 坐标 系 间 的 变换 。 或 者 可 以 用 反 过 
来 的 工 _: 


Pw = £wcPc = T De. (3.41) 


FRE, TPIT. ABA AAR ANTAL A, SEEM RE 
一 个 逆 而 已 。 实 践 当 中 使 用 T 更 加 和 常见， 而 TT、 更 为 直观 。 如 果 把 上 在 
两 式 的 p, 取 成 零 问 量 ， 也 束 是 相机 坐标 系 中 的 原点 ， 那 么 ， 此 时 的 P, wt 
是 相机 原点 在 世界 坐标 系 下 的 坐标 : 


Pw = 4wc0 = twe. (3.42) 


我 们 发 现 这 正 是 了， 的 平移 部 分 。 因 此 ， 可 以 从 T， 中 直接 看 到 相 林 
在 何 处 ， 这 也 是 我 们 说 T 更 为 直观 的 原因 。 因 此 ， 在 可 视 化 程序 里 ， 
我 们 显示 了 T ,而 不 是 T、， 

习题 

1. UE NE PEE D Re IE E SEB 

2.* 寻 找 罗 德里 格 斯 公式 的 推导 过 程 并 加 以 理解 。 


3. 验 证 四 元 数 讶 转 攻 个 点 后 ， 结 来 是 一 个 虚 四 元 数 〈 实 部 为 零 ) ， 
所 以 仍然 对 应 到 一 个 三 维 空间 点 ( 式 3.34) 。 


4. 男 表 总 结 旋转 和 矩阵 、 轴 和 角 、 欧 拉 角 、 四 元 数 的 转换 关系 。 

5. 假 设 有 一 个 大 的 Eigen 和 矩阵 ， 想 把 它 的 左上 角 3x 3 的 块 取出 来 ， 然 
后 赋值 为 T。. 。。 请 编程 实现 。 

6.* 一 般 线性 方程 Ax =b 有 哪 几 种 做 法 ? 你 能 在 Eigen 中 实现 吗 ? 

7. 设 有 小 萤 卜 一 写 和 小 萝卜 二 号 位 于 世界 坐标 系 中 。 小 莹 卜 一 号 的 


位 姿 为 9 ，=[0. 35, 0. 2,0. 3, 0. 1],t , =[0. 3, 0. 1, 0. 1. (q 的 第 一 项 为 实 
部 。 请 把 q 归 一 化 后 再 进行 计算 ) 。 这 里 的 g 和 + 表达 的 是 T o EME 
世界 坐标 系 到 相机 坐标 系 的 变换 关系 。 小 萝卜 二 号 的 位 姿 为 q ，=[- 0. 5, 
0. 4,- 0. 1, 0. 2],t =[- 0. 1, 0. 5,0. 3E 。 现 在 ， 小 蔓 卜 一 号 看 到 某 个 点 在 自 


身 的 坐标 系 下 坐标 为 p =[0. 5, 0, 0. 2]T ， 求 该 向 量 在 小 萝卜 二 号 坐标 系 下 
的 坐标 。 请 编程 实现 。 





[1] TEAC AB FE BIDS 73 E] EF Pe E IERE. 
[2] 官方 主页 : http:;//eigen.tuxfamily.org/index.php?titlezMain Page. 
[3] 感 兴趣 的 读者 请 参见 https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula。 


[4] https://en.wikipedia.org/wiki/Gimbal lock. 


第 4 讲 ” 奉 和 群 与 李 人 代数 


主要 目标 

1. 理 解 李 群 与 李 代 数 的 概念 ， 掌 握 SO(3), SE(3) 与 对 应 李 代 数 的 表示 
pa 

2. 理 解 BCH 近 似 的 意义 。 

3. 学 会 在 李 代 数 上 的 扰动 模型 。 

4. 使 用 Sophus 对 李 代 数 进行 运算 。 

上 一 讲 ， 我 们 介绍 了 三 维 世 界 中 刚体 运动 的 描述 方式 ， 包 括 旋 转 冠 
阵 、 旋 转 同 量 、 欧 拉 角 、 四 元 数 等 春 干 种 方式 。 我 们 重点 介绍 了 旋转 的 
表示 ， 但 是 在 SLAM 中 ， 除 了 表示 之 外 ， 我 们 还 要 对 它们 进行 估计 和 优 
化 。 因 为 在 SLAM 中 位 姿 是 未 知 的 ， 而 我 们 需要 解决 什么 样 的 相机 位 姿 
最 符合 当前 观测 数据 ”这 样 的 问题 。 一 种 典型 的 方式 是 把 它 构建 成 一 个 
优化 问题 ， 求 解 最 优 的 R,t ， 使 得 误差 最 小 化 。 

如 前 所 言 ， 旋 转 矩 阵 自 旱 是 禹 有 约束 的 〈 正 交 且 行列 式 为 1) o. € 
们 作为 优化 变量 时 ， 会 引入 额外 的 约束 ， 使 优化 变 得 困难 。 通 过 李 群 一 
李 代 数 间 的 转换 关系 ， 我 们 希望 把 位 姿 估 计 变 成 无 约束 的 优化 问题 ， 简 
化 求解 方式 。 考 虑 到 读者 可 能 还 没有 至 群 全 代数 的 基本 知识 ， 我 们 将 从 
最 基本 的 知识 开始 讲 起 。 
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Eo, RIINA Se Fe FEM BE PRES EM SIN, T= 
维 旋转 矩阵 构成 了 特殊 正 交 群 ”SO(3)， 而 变换 矩阵 构成 了 特殊 欧 氏 群 
SE(3): 


SO(3) = {R € R?'^|RR! = I,det(R) = 1}. (4.1) 
t 

SE(3) = (r = ; | c R***|R € SO(3,t € d | (4.2) 
0! 1 





不 过 ， 妆 时 我 们 并 未 详细 解释 群 ” 的 信义 。 细 心 的 读者 应 该 会 注音 
到 ， 放 转 窍 阵 孔 好， 变换 官 阵 也 好 ， 它 们 对 加 法 是 不 封 轩 的“。 换 句 话 
说 ， 对 于 任意 两 个 旋转 定 阵 Ri ,R，， 按 照 矩 阵 加 法 的 定义 ， 和 不 再 旦 一 
^N EPOR PE: 


R4 + Rə H SO(3). (4.3) 


对 于 变换 矩阵 亦 是 如 此 。 我 们 发 现 ， 这 两 种 窍 阵 并 没有 恨 好 定义 的 
加 法 ， 相 对 地 ， 它 们 只 有 一 种 较 好 的 运算 : FYE. SOB)AMSEB)KTH 
法 是 封闭 的 : 

Rı Rə € SO(3), Til» € SE(3). (4.4) 

我 们 知道 ， 乘 法 对 应 着 旋转 或 变换 的 复合 ， 两 个 旋转 窍 阵 相 乘 表示 
做 了 两 次 旋转 。 对 于 这 种 只 有 一 个 运算 的 集合 ， 我 们 称 之 为 群 。 


4.1.1 和 群 
Rf (Group) 是 一 种 集合 加 上 一 种 运算 的 代数 结构 。 我 们 把 集合 


记 作 A ， 运 算 记 作 : ， 那 么 群 可 以 记 作 G =A, )。 和 群 要 求 这 个 运算 满足 以 
FILTRE: 


1. 封 闭 性 : Va,,a, <Aa,-a, SA. 

2. 结 合 律 Va,,a,,a, EA, (a,-a,ya,-a,-(a,:a,). 
3.476: da, EA,s.t.VYaEA,a,':a=a'a,=a. 

435: Va€A,da ' €As.tLaa ! -a,. 


ie A] DI ESR Ag FRAT AT DASE, AERABPERÉÜeGORORB DE 
FEV Re AHF, Fe) PEAR oe RB Me A ME TE HH PA BRE CAL Or Be an’ TI An ie 
Tee BE ERE BAR AE RERE) o SETS LE BE BLS RAINE, +), ATO 
后 的 有 理 数 的 乘法 〈 双 元 为 1) (Q\0,-), SS. FAME WIRES: 

一 般 线 性 群 GL(n ) 指 nxn 的 可 逆 和 矩阵 ， 它 们 对 卸 阵 乘法 成 群 。 

* 竺 殊 正 交 群 SO ) 也 就 是 所 谓 的 旋转 矩阵 群 ， 其 中 SO(2) 和 SO(3) 最 
Alin JLo 

“特殊 欧 氏 群 SE(n Eien EEn 维 欧 氏 变换 ， 如 SE(2) 和 
SE(3)。 

群 结构 保证 了 在 群 上 的 运算 具有 民 好 的 性 质 ， 而 群 论 则 是 研究 群 的 
各 种 结构 和 性 质 的 理论 ， 但 这 里 不 多 加 介绍 。 感 兴趣 的 读者 可 以 参考 任 
意 一 本 近世 代数 教材 。 

李 群 是 指 具 有 连续 (光滑 性质 的 群 。 像 整数 群 Z 那 样 离散 的 群 没 
有 连续 性 质 ， 所 以 不 是 李 群 。 而 SO(n”) 和 SE(n  ) 在 实数 空间 上 是 连续 
的 。 我 们 能 够 直观 地 想象 一 个 刚体 能 够 连续 地 在 空间 中 运动 ， 所 以 它们 
都 是 李 群 。 由 于 SO(3) 和 SE(3) 对 于 相机 姿态 估计 尤其 重要 ， 所 以 我 们 主 
要 讨论 这 两 个 李 群 。 如 果 读 者 对 李 群 的 理论 性 质感 兴趣 ， 请 参考 文献 
[20]。 

下 面 ， 我 们 先 从 较 简 单 的 SO(3) 开 始 讨论 ， 我 们 将 发 现 每 个 李 群 都 
有 对 应 的 李 人 代数。 我 们 首先 引出 SO(3) 上 面 的 李 代 数 so(3)。 


4.1.2 ”从 代数 的 引出 


考虑 任意 旋转 矩阵 R ， 我 们 知道 它 满足 : 


RR)! — I. (4.5) 


ME, RIH, R 是 条 个 相机 的 旋转 ， 它 会 随时 间 连 续 地 变化 ， 即 
为 时 间 的 函数 : R(t). HP E Vie eM, A 


R(t)R(t)* — I. 
在 等 式 两 边 对 时 间 求 导 ， 人 得 到 : 
RORA + R(t) R(t)! = 0. 


整理 得 : 
RORO" = - (RRO) . (4.6) 


可 以 看 出 RCR (t ) 是 一 个 反对 称 AREE. IZ — P. RIE 
(3.3) MAXL, IAT ^ 符号 ， 将 一 个 同 量 变 成 了 反对 称 窍 阵 。 
间 理 ， 对 于 任意 反对 称 窃 阵 ， 我 们 亦 能 找到 一 个 与 乙 对 应 的 问 量 。 把 这 
个 运算 用 从 号 ”表示 : 


0 —üa3 ag 
a’ = A= Q3 () 一 Q1 A" =a. (4.7) 
—ü» Q1 0 


FÆ, FF R(t)R(t) 是 一 个 反对 称 和 矩阵 ， 我 们 可 以 找到 一 个 三 维 
问 量 (t)& R? 与 乙 对 应 。 于 是 有 : 


R(t) R(t)" = $(t)^. 


Se TUPI ER (t) FAR 为 正 交 阵 ， 有 : 


R(t) = G(t^R(t) | 63 0 -4|R(t). (4.8) 
—03 Qı U 


可 以 看 到 ， 每 对 旋转 矩阵 求 一 次 导数 ， 只 需 左 乘 一 个 人 (t ERER 
可 。 为 方便 讨论 ， 我 们 设 t。=0， 并 人 设 此 时 旋转 滤 阵 为 R (0 o FEHR SAY 
定义 ， 可 以 把 R GO) 在 0 附近 进行 一 阶 泰 勒 展 开 : 
R (t) ~ R (to) + R (to) (t — to) 
= I+ ó(to)^ (t). 
我 们 看 人 到 9 反映 了 R 的 导数 性 质 ， 故 称 它 在 SO(3) 原 点 附近 的 正切 空 
间 (Tangent Space) E» [AJIT fet, 附近 ， 设 9 RENEO (t,)70 4. 31 


(4.9) 


R(t) = $(to) R(t) = $0 R(t). 


上 式 是 一 个 关于 R 的 微分 方程 ， 而 且 知 道 初始 值 R (OSI, Z, 


N 
cju 


R(t) = exp (901). (4.10) 


该 者 可 以 验证 上 式 对 微分 方程 和 初始 值 均 成 立 。 不 过 ， 由 于 做 了 一 
定 的 假设 ， 所 以 它 只 在 t =O A. BUNA, FERR 与 另 一 个 
反对 称 矩 阵 9 ,通过 指数 关系 发 生 了 联系 。 也 融 是 说 ， 当 我 们 知道 某 个 时 
刻 的 R 时 ， 和 存在 一 个 同 量 p9 ， 它 们 满足 这 个 矩阵 指数 关系。 但 是 矩阵 的 
指数 是 什么 呢 ? 这 里 我 们 有 两 个 问题 需要 泣 清 : 

1. 如 果 上 式 成 立 ， 那 么 给 定 菜 时 刻 的 R » BAERS hd € 
描述 了 R 在 局 部 的 导数 关系 。 与 R 对 应 的 9 有 什么 含义 呢 ? 后 面 会 看 
7j, o 正 是 对 应 到 SO(3) 上 的 李 代 数 so0(3); 


2. 其 次 ， 和 矩阵 指数 exp(9 人 人 ”) 如 何 计算 ? 事实 上 ， 这 正 是 李 群 与 李 代 
DUA] Ta BU xT BUR o 


下 面 一 一 加 以 介绍 。 
41.3 4 (VACHE XM 

每 个 李 群 都 有 与 之 对 应 的 李 代 数 。 李 代数 描述 了 李 群 的 局 部 性 质 。 
通用 的 李 代 数 的 定义 如 下 : 

李 代 数 由 一 个 集合 V， 一 个 数 域 F 和 一 个 二 元 运算 [， ] 组 成 。 如 果 它 
们 满足 以 下 几 条 性 质 ， 则 称 (V, FLL 了 ) 为 一 个 李 代 数 ， 记 作 g。 

1. 封 闭 性 YXYE V, [XY]E v. 

2. 双 线性 YXYZE V,abb€ F, B: 


[aX +b6Y,Z]=alX,Z|4+0/Y,Z], |Z,aX +bY]=alZ, X] - o|Z, Y]. 


3. HEP v X€ V, [X,X ]-0. 

4. 雅 可 比 等 价 YXYZE V, LX, [YZ IZ, [X,Y TITY, [Z.X ]]-0. 

其 中 二 元 运算 被 称 为 李 括 号 ”。 从 表面 上 来 看 ， 李 代数 所 需要 的 性 
质 还 是 挺 多 的 。 相 比 于 群 中 的 较为 简单 的 二 元 运算 ， 李 括号 表达 了 两 个 
元 素 的 差异 。 它 不 要 求 结合 律 ， 而 要 求 元 素 和 上 自己 做 李 括 号 之 后 为 零 的 
性 质 。 作 为 例子 ， 三 维 同 量 Ra 上 定义 的 义 积 x 是 一 种 李 括 写 ， 因 此 g= 
(R^, R,x ) 构 成 了 一 个 李 代 数 。 读 者 可 以 尝试 将 叉 积 的 性 质 代 入 到 上 面 四 
条 性 质 中 。 


4.14 ”于 代数 so(3) 


之 前 近 到 的 6 ， 事 实 上 是 一 种 他 代数 。SO(3) 对 应 的 李 人 代数 是 定义 
TER? 上 的 同 量 ， 我 们 记 作 由 。 根 据 前 面 的 推导 ， 每 个 9 者 可 以 生成 一 个 
BOOT PR AE BE 


(4.11) 


FENCE X. P. ANHEE, Q, INES A 


($1, bo] = (8, By 一 P281)“. (4.12) 
ie H UREZ E CPA SE LILA. Fo 5E 
对 称 窍 阵 关 系 很 紧密 ， 在 不 引起 上 收 义 的 情况 下 ， 束 说 so(3) 的 元 系 是 三 维 
HERE EIEEE, AKA]: 
so(3) = {9 € R^, = p^ ER”? }. (4.13) 

*eyb, RIJECE Tso )NAR. CEA EHAR 
4, SET n EOS NLSI— AP OT RFE, PURAF EE Sa. ER 
SO(3)If] KR A EH TE BAR 25 XE : 

R = exp( ^). 

BARI SER TA. HT OA ZS Tso) RIEKE 

SE(3) EXT NL BART. 


4.15 ”全 代数 se(3) 


对 于 SE(3)， 它 也 有 对 应 的 至 代数 se(3)。 为 方 省 篇 幅 ， 这 里 整 不 介 
绍 如 何 引 出 se(3) 了 。 与 So(3) 相 似 ，se(3) 位 于 Rs 空间 中 : 


(4.14) 


o^ p 


se(3) — | | | c R°, p € R^, € so(3),£^ = | an] (4.15) 
9 o 0 





我 们 把 每 个 se(3) 元 系 记 作 E&  ， 它 古 一 个 六 维 辣 量 。 前 三 维 为 平移 


(但 含义 与 变换 矩阵 中 的 平移 不 同 ， 分 析 见 后 〉》， 记 作 p ;后 三 维 为 旋 
转 ， 记 作 6 ， 实 质 上 是 so(3) 元 素 申 。 同 时 ， 我 们 拓展 了 ^ 符号 的 含义 。 
在 se(3) 中 ， 同 样 使 用 ^ 符 写 ， 将 一 个 六 维 同 量 转 换 成 四 维和 矩阵 ， 但 这 里 
不 再 表示 反对 称 : 


£^ = | 1 d c RX4. (4.16) 


我 们 仍 使 用 ^ M V IRRA E EE pE A ME RES p] Sc EJ 
关系 ， 以 保持 和 so(3) 上 的 一 致 性 。 读 者 可 以 简单 地 把 se(3) 理 解 成 < 由 一 
个 平移 加 上 一 个 so(3) 元 素 构 成 的 向 量 ”( 尽 管 这 里 的 。 ”还 不 直接 是 平 
移 ) 。 同 样 ， 人 至 代数 se(3) 环 有 类 似 于 so(3) 的 至 括 写 : 


£1, &2] = (£162 — EDEN)”. (4.17) 


ix AY RUE E AE is EE TRAE AEJ o BBN 
经 见 过 两 种 重要 的 李 代 数 so(3) 和 se(3) 了 。 


4.2 ”指数 与 对 数 映 身 
4.2.1 SO(3) 上 的 指数 映射 


现在 来 考虑 第 二 个 问题 ，exp(9^”  ) 是 如 何 计算 的 ? 它 是 一 个 矩阵 的 
MAO EFEMERA, RAMZU] (Exponential Map) 。 同 样 ， 
我 们 会 先 讨 论 so(3) 的 指数 映射 ， 再 讨论 se(3) 的 情形 。 

任意 矩阵 的 指数 映射 可 以 与 成 一 个 泰勒 展开 ， 但 是 只 有 在 收敛 的 情 
LRA SAR, Fea RE TLS. 

exp(A) = > + 

FL, Xfso(3)PfEEXu3&9 ， 我 们 亦 可 按 此 方式 定义 它 的 指数 映 

射 


A". (4.18) 


exp( $^) = >》 OT (4.19) 


n=0 


我 们 来 仔细 推导 一 下 这 个 定义 。 由 于 是 三 维 同 量 ， 我 们 可 以 定义 
它 的 模 长 和 它 的 方向 ， 分 别 记 作 9 和 a ， 于 是 有 70a 。 这 里 a 是 一 个 长 
度 为 1 的 方向 向 量 。 首 先 ， 对 于 a^ ， 有 以 下 两 条 性 质 : 


a^a^ = aal — I, (4.20) 
以 及 
a^a^a^ = —a^. (4.21) 


读者 可 以 自行 验证 上 述 性 质 。 它 们 提供 了 处 理 a^ 高 阶 项 的 方法 ， 
利用 这 两 个 性 质 ， 我 们 可 以 把 指数 映射 写成 ; 


ea) 


n! 


exp (6^) = exp (a^) = V7 


Nn 二 0 


1 T 1 4 
J A > A 2. NN ES 4 人 


1 | l 
二 = — 0*(a^* +... 
2! 3! A! 
1 1 1 1 
—u n E ZI Kc f+ Sep, + oe AA 
=aa + (à Th tg «Ja (i "Td Ta «Ja a 


= a^a^ + I -- sin0a^ — cos0a^a"^ 
= (1 — cos0)a^a^ + I + sin0a^ 
= cos 0I + (1 — cos0)aa! + sin6a^. 
最 后 得 到 一 个 似 兽 相识 的 式 子 : 
exp(0a^) = cos0I + (1 — cos0)aa! 十 sin0a^. (4.22) 
回 息 前 一 讲 内 容 ， 它 和 罗 德 里 格 斯 公式 ， 即 式 〈3.14) Qt. 
这 表明 ，so(3) 实 际 上 残 是 由 所 谓 的 旋转 同 量 ”组 成 的 空间 ， 而 指数 映射 
即 罗 德里 格 斯 公式 。 通 过 和 它们， 我 们 把 so(3) 中 任意 一 个 同 量 对 应 到 了 一 


个 位 于 SO(3) 中 的 旋转 窍 阵 。 反 之 ， 如 各 定义 对 数 上 映射， 也 能 把 SO(3) 中 
的 元 系 对 应 到 so(3) 中 : 





$= In(R)" = (X sni (R — g (4.23) 
MIRADE NPQ AR REA ARARA. ERIR, RAI 


已 经 介绍 过 如 何 根据 旋转 矩阵 计算 对 应 的 李 代 数 ， 即 使 用 式 (3.16) , 
利用 迹 的 性 质 分 别 求 解 转角 和 转轴 ， 采 用 那 种 方式 更 加 省 事 一 些 。 

现在 ， 我 们 介绍 了 指数 映射 的 计算 方法 。 旋 者 可 能 会 问 ， 指 数 映 射 
性 质 如 何 呢 ?是 否 对 于 任意 的 R 都 能 找到 一 个 唯一 的 和 ? 很 遗憾 ， 指 数 
映射 只 是 一 个 满 射 。 这 意味 着 每 个 SO(3) 中 的 元 素 ， 都 可 以 找到 一 个 
so(3) 元 素 与 之 对 应 ; 但 是 可 能 存在 多 个 so(3) 中 的 元 素 ， 对 应 到 同一 个 
SO(3)。 至 少 对 于 旋转 角 6 ， 我 们 知道 多 转 360 ”和 没有 转 是 一 样 的 一 一 
它 上 只 有 周期 性 。 但 是 ， 如 果 我 们 把 旋转 角度 固定 在 4r 之 间 ， 那 么 李 群 


和 人 代数 元 系 是 一 一 对 应 的 。 


SO(3) 与 so(3) 的 结论 似乎 在 我 们 的 草料 乙 中 。 它 和 我 们 前 面 讲 的 旋 
转 回 量 与 旋转 定 阵 很 相似 ， 而 指数 映射 即 羡 多 德里 格 其 公式。 旋转 定 阵 
的 导数 可 以 由 旋转 问 量 指定 ， 指 导 看 如 何在 旋转 矩阵 中 进行 做 积 分 运 
算 。 


42.0 ”SE(3) 上 的 指数 映射 


下 和 面 介绍 se(3) 上 的 指数 映 册 。 为 了 市 管 般 幅 ， 我 们 不 再 像 so(3) 那 样 
详细 推导 指数 映射 。se(3) 上 的 指数 映射 形式 如 下 : 








Aayan N 1 A\n 
exp (£^) = 24 nO") 24 win r? (4.24) 
0! 1 
^ sper (4.25) 
of 1 





"T RAUS. HURE so EB dE. TRexpXtir 28 wR IT 
推导 此 式 。 从 结果 上 看 ，E BRAUN A EAR 是 我 们 熟知 的 SO(3) 中 
的 元 素 ， 与 se(3) 当 中 的 旋转 部 分 由 对 应 。 而 右上 角 的 J 则 可 整理 为 ( 设 9 
=0a ) - 








in O in 0 ] 一 0 
J= — T A (i — =; aa’ + ——a". (4.26) 


A m RASA HERI, (ANSE HE. RES CHE 
部 分 经 过 指数 映射 之 后 ， 肥 生 了 一 次 以 J WN RAUBER TERIA. VAIS 
者 重视 这 里 的 卫 ， 因 为 后 面 还 要 用 到 。 

同样 地 ， 虽 然 我 们 也 可 以 类 比 推 得 对 数 映 射 ， 不 过 根据 变换 矩阵 了 
求 so(3) 上 的 对 应 回 量 也 有 更 省 事 的 方式 从 左上 角 的 R TEST In] 


t= J 5. (4.27) 


由 于 J 可 以 由 $ 得 到 ， 所 以 这 里 的 p 处 可 由 此 线性 方程 解 得 。 现 在 ， 


我 们 已 经 弄 清 了 储 群 、 李 代数 的 定义 与 相互 的 转换 关系 ， 忌 结 如 图 4-1 
所 示 。 如 来 读 痢 有 哪里 不 明白 ， 可 以 翻 回去 几 页 看 看 公式 推导 。 


三 维 旋转 


exp(gw)=cosg1+ (1- cos0) au +singw 指数 映射 








eb ee puh É B 
对 数 映射 | — pce z Rasa 
三 维 变换 
\\_|exp( 办 上 Jp 
exp(é - | | 
0 ] 
_ sing sin \ ;, 1-cosO , 
du de «1-32 as t. ^ 指数 映射 
一 
tr( R)-1 
AZURI ^ 0 = arccos i ) Ra=a t=Jp 








图 4-1 SO(3), SE(3), so(3), se(3) 的 对 应 关系 。 


43 ” 李 人 代数 求 寻 与 扰动 模型 
43.4 BCH 公 式 与 近似 形式 


使 用 李 代 数 的 一 大 动机 是 进行 优化 ， 而 在 优化 过 程 中 导数 是 非常 必 
要 的 信息 (我 们 会 在 第 6 讲 详 细 介 绍 ) 。 下 面 来 考虑 一 个 问题 。 虽 然 我 
们 已 经 清楚 了 SO(3) 和 SE(3) 上 的 至 群 与 衬 代 数 关 系 ， 但 是 ， 当 在 SO(3) 
中 完成 两 个 矩阵 乘法 时 ， 李 代数 中 so(3) 上 发 生 了 什么 改变 呢 ?” 反 过 来 
说 ， 当 so(3) 上 做 两 个 李 代 数 的 加 法 时 ，SO(3) 上 是 否 对 应 着 两 个 矩阵 的 
FEAR? MIRAI, WAF: 


exp ($1) exp (62) = exp ((f1 + $2) ). 
WR , ,9 , 为 标量 ， 那 显然 该 式 成 立 ; 但 此 处 我 们 计算 的 是 矩阵 的 
指数 函数 ， 而 非 标量 的 指数 。 换 言 之 ， 我 们 在 研究 下 式 是 否 成 立 : 
In (exzp(4)exp(B)) = A+B? 
很 遗憾 ， 该 式 在 矩阵 时 并 不 成 立 。 
两 个 李 代 数 指数 映射 乘积 的 完整 形式 ， 由 Baker-Campbell-Hausdor 


ff 公式 (BCH 公式 ) 外 给 出 。 由 于 其 完整 形式 较 复 森 ， 我 们 只 给 出 其 展 
FSH Bl J Lil: 


In (exp (A) exp(B)) = A+ Bt 5 |A, B] + = |A,|A, B]| — = |B, |A, B||+--- (4.28) 


其 中 口 为 李 括 号 。BCH 公 式 告 诉 我 们 ， 当 处 理 两 个 矩阵 指数 之 积 
时 ， 它 们 会 产生 一 些 由 李 括 亏 组 成 的 余 项 。 特 别 地 ， 考 虑 SO(3) 上 的 李 
代数 In (exp (62) exp (62) ".. “4h , B 为 小 量 时 ， 小 量 二 次 以 上 的 项 都 可 以 被 
忽略 掉 。 此 时 ，BCH 拥 有 线性 近似 表达 : 


Ilp) pi da. Kp AN, 
J,(o1) b2+61 s XN. 


In (exp ($2) exp ($9)) = | (4.29) 


以 第 一 个 近似 为 例 。 该 式 告诉 我 们 ， 当 对 一 个 旋转 矩阵 R，。( 李 代数 
为 6 ，) 左 乘 一 个 微小 旋转 和 矩阵 R ，( 李 代数 为 9 ，) 时 ， 可 以 近似 地 看 
E, FURAN Re, En Ef —39J,(0,):!0,. HE, 98 — 3x1 
述 了 右 乘 一 个 微小 位 移 的 情况 。 于 是 ， 李 代数 在 BCH 近 似 下 ， 分 成 了 左 
Le 近似 两 种 ， 在 使 用 时 我 们 须 注意 使 用 的 是 左 滋 模型 还 是 右 

本 书 以 左 乘 为 例 。 左 乘 BCH 近 似 雅 可 比 J 事实 上 吏 是 式 〈4.26) 的 


内 容 : 





in 0 in 0 ] 一 0 
= J = = I--|1- E aa! 十 Ly i (4.30) 
0 0 0 
ERDEN 
0 0 0 0 0 
-1 09 ad ILI d E. — x 
J, = 2 cot 51+ (i 5 cot 5 ) aa ja l (4.31) 


I 4a SRE AREY LCM r8 G EE y BY n] s 
J.(6) = Ji(-9). (4.32) 
这 样 ， 我 们 吏 可 以 谈论 李 群 乘法 与 李 代 数 加 法 的 天 系 了 。 为 了 方便 
谈 者 理解 ， 我 们 重新 叙述 一 人 BCH 近 似 的 意义 


(EE XT FES FER ， 对 应 的 李 代 数 为 。 我 们 给 它 天 和 滋 一 个 微小 旋 
te, WEAR ， 对 应 的 李 代 数 为 Ag 。 那 么 ， 在 李 群 上 ， 得 到 的 结果 就 是 
ARR ， 而 在 李 代 数 上 上， 根据 BCH 近 似 ， 为 万 !(%)As+w。 合 并 起 来 ， 可 以 
简单 地 写成 : 


exp (Ag Jexp ($^) = exp ((6 + J, ($) A)" ). (4.33) 


反之， 如 果 我 们 在 本 代数 上 进行 加 法 ， 让 一 个 9 加 上 Ab ， 那 么 可 以 
AULAE RE Ei Ze T HI EGIT] SIE: 
exp ((@ + A)^) = exp ((JiAQ)^) exp ($^) = exp (p^) exp ((J,Ad)”) (4.34) 
这 就 为 之 后 李 代 数 上 做 微 积 分 提供 了 理论 基础 。 同 样 地 ， 对 于 
SE(3)， 亦 有 类 似 的 BCH 近 似 公式 : 


exp (A£^) exp (£^) ~ exp ((7;'A€ 十 e^) | (4.35) 
exp (£^) exp (A&^) e exp ((77! A£ + €)" ). (4.36) 


xx HJ, 形式 比较 复业 ， 它 是 一 个 6x 6 的 矩阵 ， 读 者 可 以 参考 文献 [6] 


中 却 (7.82) 和 (7.83) 的 内 容 。 由 于 我 们 在 计算 中 不 用 到 该 雅 可 比 ， 故 这 里 
略 去 它 的 实际 形式 。 


4.3.2 SOBERA ERR F 


REDRESS I8] 
题 。 该 问题 有 很 强 的 实际 背景 。 在 SLAM 中 ， 我 们 要 估计 一 个 相机 的 位 
兽 和 姿态 ， 访 位 姿 是 由 SO(3) 上 的 旋转 矩阵 或 SE(3) 上 的 变换 矩阵 插 述 
Ho ATE TY AID RAI AT 。 它 观 穴 到 了 一 个 世界 坐标 位 于 
p 的 点 ， 产 生 了 一 个 观测 数据 z 。 那 么 ， 由 坐标 变换 关系 知 : 


z=Tp+w. (4.37) 


然而 ， 由 于 观测 噪声 w 的 存在 ，z 往往 不 可 能 精确 地 满足 z =Tp 的 天 
系 。 所 以 ， 我 们 通 单 会 计算 理想 的 观测 与 实际 数据 的 误差 : 


e = z — Tp. (4.38) 


假设 一 共有 N 个 这 样 的 路 标点 和 观测 ， 于 是 就 有 N 个 上 式 。 那 么 ， 
对 小 柬 下 的 位 姿 估 计 ， 相 当 于 是 寻找 一 个 最 优 的 7 ， 使 得 整体 误 关 最 小 
化 : 


N 
2 
min JP) 3 lz: = Tpslls (4.39) 


求解 此 问题 ， 需 要 计算 目标 函数 J 关于 变换 矩阵 了 的 导数 。 我 们 把 
有 具体 的 算法 留 到 后 面 再 讲 。 这 里 重点 要 说 的 是 ， 我 们 经 常会 构建 与 位 姿 
有 关 的 函数 ， 然 后 讨论 该 函数 关于 位 姿 的 导数 ， 以 调整 当前 的 估计 值 
。 然 而 ，SO(3)， SE(3) 上 并 没有 展 好 定义 的 加 法 ， 它 们 只 是 群 。 如 末 我 
们 把 了 当成 一 个 普通 矩阵 来 处 理 优化 ， 那 束 必 须 对 它 加 以 约束 。 而 从 李 
代数 角度 来 说 ， 由 于 李 代 数 由 向 量 组 成 ， 具 有 良好 的 加 法 运算 。 因 此 ， 
使 用 他 代数 解决 求 导 问题 的 思路 分 为 两 种 : 

1. 用 他 代数 表示 姿态 ， 然 后 根据 他 代数 加 法 来 对 字 代 数 求 导 。 

2. 对 至 群 左 乘 RAR 微小 扰动 ， 然 后 对 该 扰动 求 导 ， 称 为 左 扰动 
和 右 扰 动 模型 。 

第 一 种 方式 对 应 到 李 代 数 的 求 导 模型 ， 而 第 二 种 则 对 应 到 扰动 柑 
型 。 下 面 来 讨论 这 两 种 思路 的 异同 。 


4.3.3. FRROK 


首先 ， 考 虑 SO(C3) 上 的 情况 。 假 设 我 们 对 一 个 空间 点 p 进行 了 旋转 ， 
得 到 了 Rp  。 现 在 ， 要 计算 旋转 之 后 点 的 坐标 相对 于 旋转 的 导数 ， 我 们 
不 严 课 地 记 为 加 : 
ð (Rp) 
OR ` 
由 于 SO(3) 没 有 加 法 ， 所 以 该 导数 无 法 按照 导数 的 定义 进行 计算 。 
设 尺 对 应 的 李 代 数 为 ， 我 们 转 而 计算 : 


o (exp ($^) p) 
Bp ` 





按照 导数 的 定义 ， 有 月 : 


alep (HP) | exp((ó--09)")p — exp (ó^)p 
Od i 0$—0 Op 

— jm SR) ) exp (6^) p — exp (6^) p 

i 0 一 0 Op 

- (I + (Ji60)^) exp (6^) p — exp ($^) p 

2 m = 
0 一 0 od 

— dim IO exp (o^) p 

| 660 dd 

T —-(exp($^)p)^Jióo _ ^ 
SA a —— cU. Jy. 


第 2 行 的 近似 为 BCH 线 性 近似 ， 第 3 行为 泰勒 展开 舍 去 高 阶 项 后 的 近 
似 ， 第 4 行 至 第 5 行将 反对 称 符 号 看 作 又 积 ， 区 换 之 后 变 亏 。 于 是 ， 我 们 
推导 出 了 旋转 后 的 点 相对 于 李 代 数 的 导数 : 


= (- Rp) Ji. (4.40) 





不 过 ， 由 于 这 里 仍然 食 有 形式 比较 复 森 的 ， 我 们 不 六 和 希望 计算 
它 。 而 下 和 面 要 讲 的 扰动 模型 则 提供 了 更 简单 的 导数 计算 方式 。 


43.4 HIRE ER) 


男 一 种 求 导 方 式 ， 是 对 R 进行 一 次 扰动 AR 。 这 个 扰动 可 以 乘 在 左 
边 也 可 以 乘 在 右边 ， 最 后 结束 会 有 一 氮 儿 微小 的 兰 异 ， 我 们 以 元 扰动 为 
例 。 设 左 扰 动 AR 对 应 的 本 代数 为 p 。 然 后 ， 对 gq KTF, BH: 


oq EXPE”) exp (9^) p — exp (^) p. m 
Oo Q0 p 


该 式 的 求 寻 比 上 面 更 为 简单 : 





AL, ALCS Bete BOOKS, AA ERT EL, BIER. B 
使 得 扰动 模型 更 为 实用 。 请 读者 务必 理解 这 里 的 求 导 运算 ， 


ð (Rp) 





= ji 
Ov” wp 一 0 p 
~ jim Ht e)exp(6^)p - erp) p 
q—0 p 
^ u ^ 
q—0 p Q0 p 


计 当中 具有 重要 的 意义 。 
4.3.5 SE(3) 上 的 李 代 数 求 导 


最 后 ， 我 们 给 出 SE(3) 上 的 扰动 模型 ， 而 耳 接 他 代 数 上 的 求 导 束 个 
HMAT. BRETH Ap ”经 过 一 次 变换 了 


数 为 65 =[6p,69 |", AA: 














exp (06^) exp (£^) p — exp (£^) p 


568 一 0 0€ 
ug, M t 98^) exp (E^) p — exp (£^) p 
I 一 = 一 = 一 = 
ó£—0 ÔE 
A A 
lim 98 p (E`) P 
ó£—0 ÒE 
6d op Rp+t 
| o° 0 1 
ÔE 
ôp^ (Rp + t) - óp 
0 
samy ÔE m 


exp (v^) exp (p^) p — exp (p^) p 


—(Rp +t)” 
074 


这 在 位 姿 估 


(对 应 至 代数 为 € ) ， 得 
到 Tp !9 。 现 在 ， 给 T 左 乘 一 个 扰动 AT -exp(ó£^ )， 我 们 设 扰动 项 的 李 代 


| 2 (Tp)®. 


我 们 把 最 后 的 结果 定义 成 一 个 算 从 中 ， 它 把 一 个 齐 次 坐标 的 空间 
点 变换 成 一 个 4x BAY FEE 

至 此 ， 我 们 已 经 介绍 了 奉 群 李 代 数 上 的 微分 运算 。 之 后 的 章节 中 ， 
我 们 将 应 用 这 些 知识 去 解决 实际 问题 。 关 于 李 群 李 代 数 的 某 些 重要 数学 
性 质 ， 我 们 作为 习题 留 给 读者 。 


4.4 实践 : Sophus 


我 们 已 经 介绍 了 至 代数 的 入 门 知 识 ， 现 在 是 通过 实践 演练 巩 国 一 下 
所 学 知识 的 机 会 了 。 我 们 来 讨论 如 何在 程序 中 操作 全 代数。 在 第 3 讲 
中 ， 我 们 看 到 Eigen 提 供 了 几何 模块 ， 但 没有 提供 全 代数 的 支持。 一 个 
较 好 的 李 代 数 库 是 Strasdat 维 护 的 Sophus 库 中 。Sophus 库 支持 本 章 主要 讨 
论 的 SO(3) 和 SE(3)， 此 外 还 含有 二 维 运动 SO(2)， SE(2) 以 及 相似 变换 
Sim(3) 的 内 容 。 它 是 直接 在 Eigen 基 础 上 开发 的 ， 我 们 不 需要 安装 人 额外 的 
KINE. A A U HIMA GitHub EIR IX Sophus ， 或 者 ， 在 本 书 的 代码 
目录 slambook/3rdparty 下 也 提供 了 Sophus 源 代码 。 由 于 历史 原因 ， 
Sophus 早 期 版 本 只 提供 了 双 精 度 的 宇 群 /全 代数 类 。 后 续 版 本 改 瑟 成 了 
柑 板 类 。 标 板 类 的 Sophus 中 可 以 使 用 不 同 精度 的 他 群 /他 代数 ， 但 同时 
增加 了 使 用 难度 。 本 书 使 用 非 模板 类 ”的 Sophus 库 。 如 果 读 者 准备 使 用 
GitHub 上 的 Sophus， 请 确保 使 用 的 是 非 模 板 交 ” 的 版 本 。 你 可 以 输入 以 
下 命令 获得 非 模 板 类 的 Sophus: 


1 | git clone https://github.com/strasdat/Sophus.git 
2 |cd Sophus 
3 | git checkout a621ff 


本 书 的 3rdparty 中 提供 的 Sophus 也 是 非 模 板 版 本 。Sophus 本 和 吴杰 是 
一 个 cmake 工 程 。 想 必 你 已 经 了 解 如 何 编 译 cmake 工 程 了 ， 这 里 不 再 次 
述 。Sophus 库 只 第 编 详 即 可 ， 无 须 安 宅 。 

下 面 来 演示 一 下 Sophus 库 中 的 SO(3) 和 SE(3) 运 算 

四 slambook/ch4/useSophus/useSophus.cpp 


#include <iostream> 


#include <cmath> 


using namespace std; 


#include <Eigen/Core> 


#include <Eigen/Geometry> 


#include "sophus/so3.h" 


#include "sophus/se3.h" 


int main( int argc, char** argv ) 


{ 


// i& Z 轴 转 90 RW we EE 
Eigen: :Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen: :Vector3d(0,0,1)). 


toRotationMatrix() ; 

Sophus::S03 S03 R(R); // Sophus: :SU (3) * VA 3L 4& A. we Fe 4e TE HE 
Sophus::S03 S03, v( 0, 0, M PI/2); Z 亦 可 从 旋转 向 量 构造 
Eigen::Quaterniond q(R); // 或 者 四 元 数 


Sophus::S03 S03 q( q ); 

// 上 述 表 达 方 式 都 是 等 价 的 

// 输出 S0(3) 时 ， 以 so(3) 形式 输出 
cout««"S0(3) from matrix: "««S03 R««endl; 
cout<<"S0(3) from vector: "««S03 v««endl; 
cout<<"S0(3) from quaternion :"<<S03_q<<end1; 


// 使 用 对 数 映射 获得 它 的 李 代 数 
Eigen::Vector3d so3 = $03 R.logO; 


63 


64 


cout<<"so3 = "<<so3.transpose()<<endl; 

// hat 为 向 量 到 反对 称 和 矩阵 

cout<<"so3 hat="<<Sophus: :S03: :hat (so3)<<end1; 

// 相对 地 ， vee 为 反对 称 到 向 量 

cout<<"so3 hat vee= "<<Sophus::S03::vee( Sophus::S03::hat(so3) ).transpose()«« 
endl; 


// 增 量 扰动 模型 的 更 新 

Eigen::Vector3d update_so3(1e-4, 0, 0); // 假设 更 新 量 有 这 么 多 

Sophus::S03 S03 updated = Sophus::S03::exp(update_so3)*S03_R; // Ak f$ 
cout««"S03 updated = "««S03 updated««endl; 


os ER Ee i) 3j 的 分割 线 eooleleeloereroloreoreleloreolereroreieeieloee/ 
COut««'ooololololookookek. FOE 分 E] £X, kek RRR RK <<endl ; 


// 对 SE(3) 操作 大 同 小 并 


Eigen::Vector3d t(1,0,0); // 沿 X 轴 平 移 1 
Sophus::SE3 SE3 Rt(R, t); // 从 R,t 构造 SE(3) 
Sophus: :SE3 SE3 qt(q,t) ; // 从 q,t 构造 SE(3) 


cout««"SE3 from R,t= "««endl««SE3 Rt««endl; 

cout<<"SE3 from q,t= "««endl««SE3 qt««endl; 

// 李 代 数 se(3) 是 一 个 六 维 向 量 ， 方便 起 见 先 typedef 一 下 

typedef Eigen::Matrix<double,6,1> Vector6d ; 

Vector6d se3 = SE3_Rt.log(); 

cout<<"se3 = "««se3.transpose()««endl; 

// XA B. SAME Sophus T, se(3) 平移 在 前 ， 旋 转 在 后 。 与 我 们 的 书 中 是 一 臻 
的 。 

// 同样 地 ， 有 hat 和 vee 两 个 运算 符 

cout<<"se3 hat = "<<endl<<Sophus: :SE3: :hat (se3)<<end1; 

cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<< 
endl; 


// 最 后 ， 演 示 一 下 更 新 

Vector6d update_se3; // 更 新 量 

update se3.setZero(); 

update se3(0,0) = 1e-4d; 

Sophus::SE3 SES3 updated = Sophus::SE3::exp(update se3)*SES3 Rt; 
cout««"SE3 updated = "««endl««SE3 updated.matrix()««endl; 


return 0; 


该 尖 示 程序 分 为 两 部 分 。 前 半 部 分 介绍 SO(3) 上 的 操作 ， 后 半 部 分 
则 为 SE(3)。 我 们 演示 了 如 何 构 造 SO(3)，SE(3) 对 象 ， 对 它们 进行 指数 、 
对 数 映 射 ， 以 及 当知 道 更 新 量 后 ， 如 何 对 李 和 群 元 素 进 行 更 新 。 如 果 该 者 
切实 理解 了 本 讲 内 容 ， 那 么 这 个 程序 对 你 来 说 应 该 没有 什么 难度 。 为 了 
编译 它 ， 请 在 CMakeLists.txt 里 添加 以 下 几 行 ; 


由 slambook/ch4/useSophus/CMakeL ists.txt 


# 为 使 用 sophus ， 需 要 使 用 find package 命令 找到 它 
find_package( Sophus REQUIRED ) 
include directories( $íSophus INCLUDE, DIRS) ) 


s | add executable( useSophus useSophus.cpp ) 
target link libraries( useSophus $í(Sophus, LIBRARIES) ) 





fi nd_package 命 令 是 cmake 提 供 的 寻找 未 个 库 的 头 文件 与 库 文 件 的 
指令 。 如 采 cmake 能 够 找到 它 ， 了 驶 会 提供 头 文 件 和 库 文 件 所 在 的 目录 的 
变量 。 在 Sophus 这 个 例子 中 ， 束 是 Sophus_INCLUDE_DIRS 和 
Sophus_LIBRARIES 这 两 个 变量 。 根 据 它们 ， 我 们 就 能 将 Sophus 库 引入 
目 己 的 cmake 工 程 了 。 请 读者 目 行 但 看 此 程序 的 输出 信息 ， 它 与 我 们 之 
前 的 推导 十 一 致 的 。 


45 *#* 相 似 变 换 群 与 李 代 数 


最 后 ， 我 们 要 提 一 下 在 单 目 视 党 中 使 用 的 相似 变换 群 Sim(3)， 以 及 
对 应 的 李 代 数 sim(3)。 如 果 你 只 对 双 目 SLAM 或 RGB-D SLAM 感 兴趣 ， 
可 以 跳 过 本 节 。 

我 们 已 经 介绍 过 单 目的 尺度 不 确定 性 。 如 果 在 单 目 SLAM 中 使 用 
SE(3) 表 示 位 姿 ， 那 么 由 于 尺度 不 确定 性 与 尺度 深 移 ， 人 整个 SLAM 过 程 中 
的 尺度 会 发 生变 化 ， 这 在 SE(3) 中 未 能 体现 出 来 。 因 此 ， 在 单 目 情况 下 
我 们 一 般 会 显 式 地 把 尺度 因子 表达 出 来 。 用 数学 语言 来 说 ， 对 于 位 于 衬 
间 的 点 p ， 在 相机 坐标 系 下 要 经 过 一 个 相似 变换 ， 而 非 欧 氏 变 换 : 


p= sRyp - t. (4.42) 








在 相似 变换 中 ， 我 们 把 尺度 s 表达 了 出 来 。 它 同时 作用 在 p 的 3 个 举 
标 之 上 ， 对 p 进行 了 一 次 缩放 。 与 SO(3)、SE(3) 相 似 ， 相 似 变 换 亦 对 算 
阵 乘 法 构成 群 ， 称 为 相似 变换 群 Sim(3): 





€ e (4.43) 


同样 地 ，Sim(3) 也 有 对 应 的 李 代 数 、 指 数 映 射 、 对 数 映 射 等 。 李 代 
数 sim(3) 元 素 是 一 个 7 维 癌 量 5 。 它 的 前 6 维 与 se(3) 相 同 ， 最 后 多 了 一 项 o 


sim(3)= <¢\C= | o ewe P | er | (4.44) 
0 


它 比 se(3) 多 了 一 项 ac 。 关 联 Sim(3) 和 和 sim(3) 的 仍 是 指数 映射 和 对 数 映 
射 。 指 数 映 射 为 


e? exp (p^) Jsp 
exp (Ç^) = | (4.45) 
0! 1 
其 中 天 形式 为 
do 05a ee OR A 
o o? + 0? 
" e? —1  (e?cos0 — 1)o + (e? sin0)8 "3 
o* + 0* 


Wie UR, BAT Be Ue BU TRA CS PH Ro MY ZEB 
RÇ, p 群 的 对 应 天 系 为 


s= 6, Rep iJ. (4.46) 


旋转 部 分 和 SO(3) 是 一 致 的 。 平 移 部 分 ， 在 se(3) 中 需要 乘 一 个 雅 可 
FE aad: ae “对 于 尺度 因子 ， 可 以 看 到 李 群 
中 的 s 即 为 李 代 数 中 da 的 指数 函数 。 


Sim(3) 的 BCH 近 似 与 SE(3) 是 类 似 的 。 我 们 可 以 讨论 一 个 点 p 经 过 相 
似 变 换 Sp 后 ， 相 对 于 S 的 叶 数 。 同 样 地 ， 存 在 微分 模型 和 扰动 模型 两 种 
方式 ， 而 扰动 模型 较为 徐 单 。 我 们 省 略 推 寻 过 程 ， 直 接 给 出 扰动 模型 的 
结果 。 设 给 予 Sp 雹 侧 一 个 小 扰动 exp(C^ )， 并 求 Sp 对 于 扰动 的 导数 。 
为 Sp 是 4 维 的 齐 次 坐标 ，” 是 7 维 问 量 ， 访 导数 应 该 是 4x 7 的 雅 可 比 。 为 
了 方便 起 见 ， 记 Sp 的 前 3 维 组 成 回 量 q ， 那 么 


OSp I —q^ q 
一 一 一 = 4.47 
s | J iun 


关于 Sim(3)， 我 们 就 介绍 到 这 里 。 更 详细 的 关于 Sim(3) 的 资料 ， 建 
议 谈 者 参见 文献 [21]。 


4.6 ”小 结 


本 讲 引 入 了 李 群 SO(3) 和 SE(3)， 以 及 它们 对 应 的 李 代 数 so(3) 和 
se(3)。 我 们 介绍 了 位 姿 在 它们 上 面 的 表达 和 转换 ， 然 后 通过 BCH 的 线性 
近似 ， 我 们 可 以 对 位 姿 进 行 求 导 和 扰动 了 了。 这 给 之 后 讲解 位 姿 的 优化 打 
下 了 理论 基础 ， 因 为 我 们 需要 经 常 地 对 某 一 个 位 姿 的 估计 值 进 行 调整 ， 
使 它 对 应 的 误 天 减 小 。 只 有 在 乔 清 芭 如 何 对 位 姿 进 行 调 整 和 更 新 之 后 ， 
我 们 才能 继续 下 一 步 的 内 容 。 

可 能 本 讲 的 内 容 比 较 偶 理论 化 ， 毕 葛 它 不 像 计算 机 视 党 那样 经 币 有 
好 看 的 图 所 可 以 展示 。 相 比 于 讲解 李 群 李 代 数 的 数学 教科 书 ， 由 于 我 们 
只 关心 实用 的 内 容 ， 所 以 讲 的 内 容 非 党 精简 ， 速 度 相 对 快 了 一 些 。 请 旋 
者 务必 理解 本 章 内 容 ， 它 是 解雇 后 续 许 多 问题 的 基础 ， 特 别 是 位 姿 估计 
部 分 。 

习题 

1. 验 证 SO(3)、SE(3) 和 Sim(3) 关 于 乘法 成 群 。 

2. 验 证 (Ra , Rx ) 构 成 李 代 数 。 

3. 验 证 so(3) 和 se(3) 请 足 李 代数 要 求 的 性 质 。 

4. 验 证 性 质 (4.20) 和 (4.21) 。 

5. 证 明 : 

Rp^ R! = (Rp)^. 

6.uE BH: 

R exp(p^) R' = exp((Rp)^). 


该 式 称 为 SO(3) 上 的 伴随 性 质 。 同 样 地 ， 在 SE(3) 上 亦 有 伴随 性 质 : 


T exp(£^)T ! = exp ((Ad(T)E€)") , (4.48) 


(4.49) 
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8. 搜 索 cmake 的 fi nd_package 指 令 是 如 何 运 作 的 。 它 有 哪些 可 选 的 
参数 ? 为 了 让 cmake 找 到 某 个 库 ， 需 要 哪些 先决 条 件 ? 





[1] 谐音 “凤姐 咬 你 ”。 

[2] 目 反 性 是 指 目 己 与 目 己 的 运算 为 零 。 

[3] 请 注意 有 些 地 方 把 旋转 放 前 面 ， 平 移 放 后 面 ， 也 是 可 行 的 。 

[4] 参见 https:/en.wikipedia.org/wiki/Baker-Campbell-Hausdorff formula. 


[5] 请 注意 这 里 并 不 能 按照 矩阵 微分 来 定义 导数 ， 这 只 是 一 个 记号 。 同 时 ， 在 后 文 极限 运算 中 ， 
严谨 的 写法 需要 对 分 母 进行 转 置 ， 才 能 得 到 现在 的 结 末 。 这 里 为 方便 书 与 故 省 去 。 


[6] 请 注意 为 了 使 乘法 成 立 ，P 必须 使 用 齐 次 坐标 。 
[7] 我 会 读 作 “ 噬 ”*"， 像 一 个 石子 挥 在 井 里 的 声 首 。 
[8] 最 早 提 出 李 代 数 的 是 Sophus Lie， 这 个 库 就 以 他 的 名 字 命 名 了 。 


[9] https://github.com/strasdat/Sophus. 


out 相机 与 图 像 


主要 目标 

1. 理 解 针 扎 相机 的 模型 、 内 参与 径 同 畸变 参数 。 
2. 理 解 一 个 空间 点 是 如 何 投影 到 相机 成 像 平 面 的 。 
3. 掌 握 OpenCV 的 图 像 存储 与 表达 方式 。 

4. 学 会 基本 的 摄像 头 标 定 方法 。 


前 面 两 讲 中 ， 我 们 介绍 了 “机 人 如 人 如 何 表示 目 喘 位 姿 * 的 问题 ， 部 分 
地 解释 了 SLAM 经 典 模型 中 变量 的 售 义 和 运动 方程 部 分 。 本 讲 将 讨 
论 “ 机 右 人 如 何 观 测 外 部 世界 ”， 也 束 古 观测 方程 部 分 。 而 在 以 相机 为 主 
的 视觉 SLAM 中 ， 观 测 主要 是 指 相 机 成 像 的 过 程 。 


我 们 在 现实 生活 中 能 看 到 大 量 的 照片 。 在 计算 机 中 ， 一 张 照片 由 很 
多 个 像 系 组 成 ， 每 个 像 系 记 录 了 色彩 或 完 度 的 信息 。 三 维 世界 中 的 一 个 
物体 反射 或 肥 出 的 光线 ， 罕 过 相机 交心 后 ， 投 影 在 相机 的 成 像 平面 上 。 
相机 的 感光 大 件 接收 到 光线 后 ， 产 生 测 量 信 ， 束 得 到 了 像 系 ， 形 成 了 我 
们 见 到 的 照片 。 这 个 过 程 能 合用 数学 原理 来 揪 述 呢 ? 本 讲 将 自 完 讨论 相 
机 模型 ， 说 明 投 影 关 系 具 体 如 何 朱 述 ， 相 机 的 内 参 是 什么 。 同 时 ， 人 简单 
介绍 双 目 成 像 与 RGB-D 相 机 的 原理 。 然 后 ， 介 绍 二 维 照 记 像素 的 基本 操 
作 。 最 后 ， 根 据 内 外 参数 的 售 义 ， 尘 示 一 个 后 云 拼接 的 实验 。 





5.1 ”相机 模型 


相机 将 三 维 世 界 中 的 坐标 点 (单位 为 米 ) 映 映 到 二 维 图 像 平 面 ( 单 
DIARIO 的 过 程 能 够 用 一 个 几何 模型 进行 手 述 。 这 个 模型 有 很 多 种 ， 
其 中 最 简 早 的 称 为 针 孔 模型 ”。 竺 和 孔 模 型 是 很 音 用 而 且 有 效 的 模型 ， 它 
摘 述 了 一 束 光 线 通 过 针 孔 之 后 ， 在 针 和 也 背面 投影 成 像 的 天 系 。 在 本 书 中 
我 们 用 一 个 简单 的 针 孔 相机 模型 来 对 这 种 映射 关系 进行 建 模 。 同 时 ， 由 
于 相机 镜头 上 的 透镜 的 存在 ， 使 得 光线 投影 到 成 像 平 面 的 过 程 中 会 产生 
ZE 。 因 此 ， 我 们 使 用 针 筷 和 晒 变 两 个 模型 来 拍 述 整个 投影 过 程 。 

在 本 市 和 完 给 出 相机 的 针 孔 模型 ， 表 对 透镜 的 困 变 模型 进行 讲解 。 这 
两 个 模型 能 够 把 外 部 的 三 维 氮 投影 到 相机 内 部 成 像 平 面 ， 构 成 相机 的 内 
参数 。 


5.1.1 针 孔 相机 模型 


在 初中 物理 诛 特 上 ， 我 们 可 能 都 见 过 一 个 旺 烛 投影 实验 : 在 一 个 暗 
箱 的 前 方 族 着 一 文 所 人 燃 的 晓 烛 ， 昱 煌 的 光 透 过 上 暗箱 上 的 一 个 小 孔 投 影 在 
音箱 的 后 方 平 面 上 ， 并 在 这 个 平面 上 形成 一 个 倒立 的 晤 烛 图 像 。 在 这 个 
过 程 中 ， 小 筷 模 型 能 够 把 三 维 世 界 中 的 暑 烛 投影 到 一 个 二 维 成 像 平 面 。 
间 理 ， 我 们 可 以 用 这 个 简单 的 模型 来 解释 相机 的 成 像 过 程 ， 如 图 5-1 所 
不 。 






小 孔 成 像 醒 型 





k VJ — LAI 
相机 坐标 系 O-x-y- ^ 相似 三 角形 


图 5-1 ” 针 孔 相机 模型 


现在 来 对 这 个 简单 的 针 孔 模型 进行 几何 建 模 。 设 O-x-y-z “为 相机 举 
标 系 ， 习 惯 上 我 们 让 z 轴 指 同 相 机 前 方 ，x pA. y I he O 为 摄像 机 的 
交心 ， 也 是 针 孔 模型 中 的 针 孔 。 现 实 世 界 的 空间 点 P ， 经 过 小 孔 O 投影 
之 后 ， 洲 在 物理 成 像 平 面 O -x -y 上， 成 像 点 为 P . WP 的 坐标 为 [X,Y,Z 
1T，P 为 [X ,Y ,Z 1， 并 且 设 物理 成 像 平 面 到 小 孔 的 距离 为 FF EE) 。 
那么 ， 根 据 三 角形 相似 关系 ， 有 : 

Z X Y 
fF- (5.1) 

其 中 负 号 表示 成 的 像 是 倒立 的 。 为 了 简化 模型 ， 我 们 可 以 把 成 像 平 
面 对 称 到 相机 前 方 ， 和 三 维 空 间 点 一 起 放 在 摄像 机 坐标 系 的 同一 人 出， 如 
图 5-2 的 中 图 所 示 。 这 样 做 可 以 把 公式 中 的 负 号 去 掉 ， 使 式 子 更 加 简 


WE 
1H: 


(3.2) 





m— — 归 一 — 
图 5-2 ”真实 成 像 平面 ， 对 称 成 像 平 面 ， 归 一 化 成 像 平 面 的 图 示 。 
整理 得 : 
_ pX 
-f7 (5.3) 
Y'— f 


谈 者 可 能 要 问 ， 为 什么 我 们 可 以 看 似 随 意 地 把 成 像 平 面 挪 到 前 方 
有 昵 ?这 只 是 我 们 处 理 真实 世界 与 相机 投影 的 数学 手段 ， 并 且 ， 大 多 数 相 
机 输出 的 图 像 并 不 是 倒 像 一 一 相机 目 映 的 软件 会 帮 你 翻转 这 张 图 像 ， 所 
以 你 看 到 的 一 般 是 正 着 的 像 ， 也 就 是 对 称 的 成 像 平 面 上 的 像 。 所 以 ， 尽 
党 从 物理 原理 来 说 ， 小 了 蕊 成 像 应 该 是 倒 像 ， 但 由 于 我 们 对 图 像 作 了 预 处 
理 ， 所 以 理解 成 在 对 称 平 面 上 的 像 并 不 会 补 来 什么 坏处 。 于 是 ， 在 不 引 
起 歧义 的 情况 下 ， 我 们 也 不 加 限制 地 称 后 一 种 情况 为 针 孔 模型 。 

式 〈5.3) 描述 了 点 P 和 它 的 像 之 则 的 空间 关系 。 不 过 ， 在 相机 中 ， 
我 们 最 终 获 得 的 是 一 个 个 的 像素 ， 这 需要 在 成 像 平 面 上 对 像 进行 采样 和 
量化 。 为 了 摘 述 传感器 将 感受 到 的 光线 转换 成 图 像 像 素 的 过 程 ， 我 们 设 
在 物理 成 像 平 面 上 国定 着 一 个 像 系 平 甸 0o-u-v 。 我 们 在 像素 平面 得 到 了 P 
的 像素 坐标 [uv]. 


像素 坐标 系 UL 通常 的 定义 方式 是 : 原点 o 位 于 图 像 的 左上 角 ，u Ht 
向 右 与 x 轴 平 行 ，v 轴 向 下 与 y 轴 平 行 。 像 素 坐 标 系 与 成 像 平面 之 间 ， 相 
差 了 一 个 缩放 和 一 个 原点 的 平移 。 我 们 设 像素 坐标 在 轴 上 缩放 了 a 
EB. tev 上 缩放 了 有 倍 。 同 时 ， 原 点 平移 了 [cc, I. 


那么 ，P 的 坐标 与 像 系 坐标 [u,v 的 关系 为 


u = AX’ + Cz 
| (5.4) 


v=BY' +c 


RAR (5.3) 并 把 of 合并 成 f ， 把 Bf 合并 成 f, ， 得 : 


MAN (5.5) 
c= f d y 

Job, f 的 单位 为 米 ，aB 的 单位 为 像素 / 米 ， 所 以 F f, BURGOS 
系 。 把 该 式 与 成 矩阵 形式 会 更 加 和 窗 洁 ， 不 过 左 侧 需 要 用 到 齐 次 坐标 : 


u to OU X 
" => 0 f, allr 全 ZKP. (5.6) 
1 0 0 1J \Z 
我 们 按照 传统 的 习惯 把 Zz 挪 到 左 侧 : 
u ls U dh X 
Zivi=lo fy e| |Y|S KP. (5.7) 
1 0 0 1) \Z 


ZAP, 3E OB IBI EZ XY E BE BCA ABLE) PI ZUR EE 
(Cameralntrinsics) K 。 通 党 认为 ， 相 机 的 内 参 在 出 厂 之 后 是 固定 的 ， 
不 会 在 使 用 过 程 中 发 生变 化 。 有 的 相机 生产 广 商会 告诉 你 相机 的 内 参 ， 
而 有 时 需要 你 目 己 确定 相机 的 内 参 ， 也 区 是 所 请 的 标定 。 鉴 于 标定 算 
法 业已 成 熟 ， 旦 网络 上 能 找到 大 量 的 标定 教学 ， 这 里 束 不 介绍 了 。 

有 内 参 ， 目 然 也 有 相对 的 外 参 。 考 虑 到 在 式 〈5.6) 中 我 们 使 用 的 
是 P 在 相机 坐标 系 下 的 坐标 。 由 于 相机 在 运动 ， 所 以 P 的 相机 坐标 应 访 
是 它 的 世界 坐标 〈 记 为 P， ) ， 根 据 相 机 的 当前 位 姿 变 换 到 相机 坐标 系 


FIA. FAL AE CER ”和 平移 癌 量 t OR GIA. ALA 


ZB,-Z|v|-K(RP,4*t-KTP,. (5.8) 


TE Ja PSSA ERE I —ÍÓTUKABERSUSEZEUKAA PRE CURE 
出 来 吗 ? ) o "EdEXR IP 的 世界 坐标 到 像素 坐标 的 投影 天 系 。 其 中 ， 相 
机 的 位 姿 R,t 又 称 为 相机 的 外 参数 (Camera Extrinsics) 。 相 比 于 不 变 的 
内 参 ， 外 参 会 随 着 相机 运动 发 生 改 变 ， 同 时 也 是 SLAM 中 竺 估计 的 目 
Pp, RAL ASA LS 

EXP A ESTA ER. KATA i Fe ESE SS RS UR EIA [A] EY 
含义 ， 所 以 可 以 简单 地 把 Z 去 挥 : 

Py, = KTP,. (5.9) 

[HIURESE ER ERSUNLAE D. MATES IAA RARER Ma, M — 
MIERE. OAS ERI, Sd eie Mu BxX EREN BHEE. 

XELLE PMS ANS IR BISEST IRN EHR. HAS, AA 
TP 表示 把 一 个 世界 坐标 系 下 的 齐 次 坐标 变换 到 相机 坐标 系 下 。 为 了 使 
EBK 相 乘 ， 需 要 取 它 的 前 三 维 组 成 同和 量 KTP, wha TEN. 
IER, AFDE, RAE AE RAER, Ea 
维 进 行 归 一 化 处 理 ， 得 到 P 在 相机 归 一 化 平面 上 的 投影 : 





X 
P. = Y = (T Pw) (1:3): P. = Y . (5.10) 
Z 


这 时 P。 可 以 看 成 一 个 二 维 的 齐 识 坐标 ， 称 为 归 一 化 坐标 。 它 位 于 
相机 前 方 z =1 处 的 平面 上 。 访 平面 称 为 归 一 化 平面 。 由 于 P, 经 过 内 参 之 


后 就 得 到 了 像素 坐标 ， 所 以 我 们 可 以 把 像素 坐标 [uv Tr 看 成 对 归 一 化 平 
面 上 的 点 进行 量化 测量 的 结 


至 此 ， 针 孔 相机 的 成 像 模 型 我 们 就 讲 清楚 了 。 
5.1.2 Haas 


JI T SRE RRR, FRAT CEAA ALY BT SR. ERRATA 
对 成 像 过 程 中 光线 的 传播 会 产生 新 的 影响 :一 是 透镜 目 身 的 形状 对 光线 
传播 的 影响 ， 二 是 在 机 械 组 装 过 程 中 ， 透 镜 和 成 像 平 面 不 可 能 完全 平 
行 ， 这 也 会 使 得 光线 穿 过 透镜 投影 到 成 像 面 时 的 位 置 发 生变 化 。 

由 透镜 形状 引起 的 畸变 称 为 径 同 畸变 ”。 在 针 筷 模型 中 ， 一 条 直线 
投影 到 像素 平面 上 还 是 一 条 直线 。 可 是 ， 在 实际 拍摄 的 照片 中 ， 摄 像 机 
的 透镜 往往 使 得 真实 环境 中 的 一 条 直线 在 图 片 中 变 成 了 曲线 中 。 越 靠近 
图 像 的 边缘 ， 这 种 现象 越 明 显 。 由 于 实际 加 工 制作 的 透镜 往往 是 中 心 对 
称 的 ， 这 使 得 不 规则 的 畸变 通常 径 向 对 称 。 它 们 主要 分 为 两 大 类 : 桶 形 
Hay AP 和 枕 形 畸变 ， 如 图 5-3 所 示 。 
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桶 形 失真 枕 形 失真 
图 5-3” 径 向 畸变 的 两 种 类 型 。 
桶 形 辐 变 是 由 于 图 像 放 大 率 随 看 与 光 轴 之 间 的 距离 增加 而 减 小 ， 而 
枕 形 辐 变 则 恰好 相反 。 在 这 两 种 晒 变 中 ， 罕 过 图 像 中 心 和 光 轴 有 交 扩 的 
直线 还 能 你 持 形 状 不 变 。 
除了 透镜 的 形状 会 引入 径 同 旺 变 外 ， 在 相机 的 组 猴 过 程 中 由 于 不 能 
使 透镜 和 成 像 面 严 榜 平行 也 会 引入 切 癌 畸变 ， 如 图 5-4 所 示 。 





摄像 头 传感器 
图 5-4 切 回 畸变 来 源 示 意图 。 


为 更 好 地 理解 径 同 困 变 和 切 同 困 变 ， 我 们 用 更 严格 的 数学 形式 对 两 
者 进行 描述 。 我 们 知道 ， 平 面 上 的 任意 一 点 P 可 以 用 人 审 卡 儿 坐 标 表示 为 
[xy 开 ， 也 可 以 把 它 写 成 极 坐 标的 形式 [6 J ， 其 中 r xm ap 与 坐标 系 
原点 之 间 的 距离 ，6 表示 与 水 平 轴 的 夹 朋 。 径 同 畸 变 可 看 成 坐标 点 沿 痢 
长 度 方 同 发 生 了 变化 er ， 也 束 是 其 距离 原点 的 长 度 发 生 了 杰 化 。 切 问 师 
ARH) DG RAB ip i eT ACE TEM, thE KAP SEARCHES ZB 
化 60 。 

对 于 径 同 畸变 —, TCE FETTER EI, BJ edle be 
2H Ej rp c ZA) A EB E AR f 3H, Dt n] EA Hd 7S S DUX LE SCR df aS B] 
变 前 后 的 坐标 变化 iXx2R08 4E HI ULF EBA APRS ACA — USC IR X 
多 项 却 图 数 进行 纠正 。 其 中 [xy 天 是 未 纠正 的 点 的 坐标 ，[X ected XY conecte 


| 古 纠 正 后 的 点 的 坐标 。 注 总 ， 它 们 都 是 归 一 化 平面 上 的 点 ， 而 不 是 
像素 平面 上 的 点 。 
Zeorrected = Z(1 + kyr? + kor* + Kar") (5.11) 
Ucorrected 一 y(1 T kyr? T kort EX kar?) 
在 陈 (5.11) 摘 述 的 纠正 模型 中 ， 对 于 畸变 较 小 的 网 像 中 心 区 域 ， 椅 
变 纠 正 主 要 是 K， 起 作用 ;而 对 于 畸变 较 大 的 边缘 区 域 ， 主 要 是 K ， 起 作 
用 。 普 通 摄像 头 用 这 两 个 系数 残 能 很 好 地 纠正 径 同 是 变 。 对 是 变 很 大 的 


摄像 头 ， 比 如 鱼 眼 镜头 ， 可 以 加 入 K , 畸变 项 对 畸变 进行 纠正 。 


另 一 方面 ， 对 于 切 向 畸变 ， 可 以 使 用 另外 的 两 个 参数 p , ,p ,来 进行 
ANTE 


Leorrected = X 十 2p LY + pa (r* * 227) (5.12) 


Ucorrected = Y + P1 (r^ " 254^) F 2p2TY 


因此 ， 联 合式 (5.11) 和 式 (5.12)， 对 于 相机 坐标 系 中 的 一 点 P (XYZ 
)， 我 们 能 够 通过 5 个 畸变 系数 找到 这 个 点 在 像 闵 平面 上 的 正确 位 置 : 


1. 将 三 维 空间 后 投影 到 归 一 化 图 像 平面 。 设 它 的 归 一 化 坐标 为 [x,y J 


2. 对 归 一 化 平面 上 的 点 进行 径 同 量变 和 切 癌 畸变 纠正 。 


- = z(1 £z kir“ T kor* sr kar9) T 2p1zy + polr? T 2^) (5.13) 


Ucorrected — y(1 E kir” T korf T kar9) T pl (r^ EE 2y7) 33 2poxy 
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上 的 正确 位 置 。 


$ = Jeaneeiad + Cy (5.14) 


Ww = f y Ucorrected 十 Cy 


在 上 面 的 纠正 畸变 的 过 程 中 ， 我 们 使 用 了 5 个 畸变 项 。 实 际 应 用 
中 ， 可 以 灵活 选择 纠正 模型 ， 比 如 只 选择 k , ,p ; ,p , 这 3 项 等 。 


在 这 一 市 中 ， 我 们 对 相机 的 成 像 过 程 使 用 针 孔 模型 进行 了 建 模 ， 也 
对 透镜 引起 的 径 问 畸变 和 切 问 盯 变 进行 了 插 述 。 实 际 的 图 像 系统 中 ， 竺 
者 们 所 出 了 很 多 其 他 的 模型 ， 比 如 相机 的 仿 射 模型 和 透视 模型 等 ， 同 时 
也 存在 很 多 其 他 英 开 的 量变 。 考 谍 到 视觉 SLAM 中 一 般 都 使 用 普通 的 摄 
像 头 ， 有 针 扎 模型 及 径 癌 困 变 和 切 同 畸变 模型 已 经 足够 ， 因 此 ， 我 们 不 
再 对 其 他 模型 进行 拍 述 。 


值得 一 提 的 是 ， 存 在 两 种 去 畸变 处 理 〈Undistort， 或 称 畸 变 校 正 ) 
做 法 。 我 们 可 以 选择 先 对 整 张 图 像 进 行 去 困 变 ， 得 到 去 畸变 后 的 图 像 ， 
然后 讨论 此 图 像 上 的 点 的 空间 位 置 。 或 者 ， 也 可 以 先 考虑 图 像 中 的 某 个 
后 ， 然 后 按照 去 量变 方程 ， 讨 论 其 去 旺 变 后 的 空间 位 置 。 二 者 都 是 可 行 
的 ， 不 过 前 者 在 视觉 SLAM 中 似乎 更 加 第 见 一 些 。 所 以 ， 当 一 个 图 像 去 
WEZ RA ERHET EREK KR, mAH A E E 
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处 理 。 


最 后 ， 我 们 小 结 一 下 单 目 相机 的 成 像 过 程 : 

1. 首 先 ， 世 界 坐 标 系 下 有 一 个 固定 的 点 P ， 世 界 坐 标 为 P，。 

2. 由 于 相机 在 运动 ， 它 的 运动 由 Rit 或 变换 矩阵 TE SE(3) 描 述 。P 的 
相机 坐标 为 B= RP, +t- 

3. 这 时 的 到 仍 有 XYZ 三 个 量 ， 把 它们 投影 到 归 一 化 平面 Zz =1 上 ， 得 
到 P 的 归 一 化 相机 坐标 : P.=[X/Z,Y/Z,1] 7"! 。 

4. 最 后 ，P 的 归 一 化 坐标 经 过 内 参 后 ， 对 应 到 它 的 像 系 坐标 : P 
-KP. . 

综 上 上 所 述 ， 我 们 一 共 谈 到 了 四 种 坐标 : 世界 坐标 、 相 机 坐标 、 归 一 
化 相机 坐标 和 像 际 坐标 。 请 读者 厘清 它们 的 关系 ， 它 反映 了 整个 成 像 的 


5.1[3 双 目 相机 模型 


针 孔 相机 模型 描述 了 单个 相机 的 成 像 模型 。 然 而 ， 仅 根据 一 个 像 
系 ， 我 们 是 无 法 确定 这 个 空间 点 的 具体 位 置 的 。 这 是 因为 ， 从 相机 交心 
到 归 一 化 平面 连 线 上 的 所 有 点 ， 都 可 以 投影 至 该 像 系 上 。 只 有 当 P 的 深 
度 确定 时 《比如 通过 双 目 或 RGB-D 相 机 ) , RAIZ SESS UJ ADS E HAE 
间 位 置 。 如 图 5-5 所 示 。 


归 一 化 平面 


zl 
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相机 坐标 系 
像素 点 P 可 能 的 位 置 


归 一 化 坐标 系 
图 5-5 ”像素 点 可 能 存在 的 位 置 。 


测量 像素 距离 (或 深度 ) 的 方式 有 很 多 种 ， 像 人 眼 就 可 以 根据 左右 
AR A SA Ace ARME) 来 判断 物体 与 我 们 之 间 的 距离 。 双 目 相 
机 的 原理 亦 是 如 此 : 通过 同步 采集 左右 相机 的 图 像 ， 计 算 图 像 间 视 关 ， 
来 估计 每 一 个 像 际 的 深度 。 下 面 简 单 讲 讲 双 目 相机 的 成 像 原 理 〈 如 图 5- 
6 所 示 ) 。 


双 目 相机 一 般 由 左 眼 相机 和 右 眼 相机 两 个 水 平 放置 的 相机 组 成 。 当 
然 也 可 以 做 成 上 下 两 个 目 和 由 ， 不 过 我 们 见 到 的 主流 双 目 都 是 做 成 左右 形 
式 的 。 在 左右 双 目 相机 中 ， 我 们 可 以 把 两 个 相机 都 看 作 针 孔 相 机 。 它 们 
是 水 平 放置 的 ， 意 味 看 两 个 相机 的 光圈 中 心 都 位 于 x 轴 上 。 两 者 之 间 的 
距离 称 为 双 目 相机 的 基线 (Baseline， 记 作 b ) ， 是 双 目 相机 的 重要 参 
BY o 





儿 何 模型 


图 5-6” 双 目 相 机 的 成 像 模型 。0 ，,O , 为 左右 光圈 中 心 ， 方 框 为 成 像 平 甸 ，f 为 焦距 。u， 和 
u ,为 成 像 平面 的 坐标 。 请 注意 ， 按 照 图 中 坐标 定义 ，u ,应 该 是 负数 ， 所 以 图 中 标 出 的 距 


房 为 -u , o 
现在 ， 考 虑 一 个 空间 点 P ， 它 在 左 眼 相机 和 右 眼 相 机 各 成 一 像 ， 记 
作 P，,P。。 由 于 相机 基线 的 存在 ， 这 两 个 成 像 位 置 古 不 同 的 。 理 想 情 况 


下 ， 由 于 左右 相机 只 在 x 轴 上 有 位 移 ， 因 此 P 的 像 也 只 在 x 轴 ( 对 应 图 
Ku D haze. Wena Au, ， 右 侧 坐 标 为 上 。 那 么 ， 其 


几何 关系 如 图 5-6 右 侧 所 示 。 根 据 APP，P， 和 和 APoOo，O， 的 相似 关系 ， 
F: 


z-f _b-ur+ur (5.15) 





z b 
稍 加 整理 ， 人 得 : 
z= i d = ur, — ug. (5.16) 


XE, d 为 左右 图 的 横 举 标 之 差 ， 称 为 视差 (Disparity) 。 根 据 祝 
差 ， 我 们 可 以 估计 一 个 像素 与 相机 之 间 的 距离 。 视 差 与 距离 成 反比 : TH 
FRA, FRR BOERS! 。 同 时 ， 由 于 视 兰 最 小 为 一 个 像 系 ， 于 是 双 目 的 深 
度 存 在 一 个 理论 上 的 最 大 值 ， 由 用 确定 。 我 们 看 到 ， 当 基线 越 长 时 ， 双 
目 能 测 到 的 最 大 距离 就 会 越 远 ;反之 ， 小 型 双 目 器 件 则 只 能 测量 很 近 的 
距离 。 

虽然 由 视差 计算 深度 的 公式 很 简洁 ， 但 视 兰 qd 本 身 的 计算 却 比 较 困 
难 。 我 们 需要 确切 地 知道 左 眼 图 像 某 个 像素 出 现在 右 眼 图 像 的 哪 一 个 位 
置 ( 即 对 应 关系 ) ， 这 件 事 亦 属 于 “人 类 觉得 容易 而 计算 机 觉得 困难 ”的 
任务 。 当 我 们 想 计算 每 个 像素 的 深度 时 ， 其 计算 量 与 精度 都 将 成 为 问 
题 ， 而 且 只 有 在 图 像 纹理 变化 丰富 的 地 方才 能 计算 视 兰 。 由 于 计算 量 的 
原因 ， 双 目 深度 估计 仍 需 要 使 用 GPU 或 FPGA 来 计算 。 这 将 在 第 13 讲 中 
提 到 。 


5.1.4 RGB-D 相 机 模型 


相 比 于 双 目 相机 通过 视 关 计算 深度 的 方式 ，RGB-D 相 机 的 做 法 更 


为 “主动 ”一 些 ， 它 能 够 主动 测量 每 个 像 系 的 深度 。 目 前 的 RGB-D 相 机 投 
原理 可 分 为 两 大 类 〈 见 图 5-7) : 


1. 通 过 红外 结构 光 (Structured Light) 来 测量 像素 距离 的 。 例 子 有 
Kinect 1 代 、Project Tango 1 代 、Intel RealSense 等 。 


2. 通 过 飞行 时 间 法 (Time-of- flight, ToF) 原理 测量 像素 距离 的 。 
例子 有 Kinect 2 代 和 一 些 现 有 的 ToF 传 感 锅 等。 


结构 光 原 理 


A NI 


飞行 时 间 原 理 
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图 5-7 RGB-D 相 机 原理 示意 图 


无 论 定 哪 种 闫 型 ，RGB-D 相 机 都 需要 回 探 训 目标 及 射 一 束 光 线 《〈 通 
WELIG) 。 在 结构 沧 原 理 中 ， 相 机 根据 返回 的 结构 区 图 案 ， 计 算 物 
体 与 自身 之 间 的 距离 。 而 在 ToF 原 理 中 ， 相 机 同 目 标 发 射 脉 冲 光 ， 然 后 
根据 肥 送 到 返回 乙 间 的 花束 飞行 时 间 ， 确 定 物体 与 目 且 之 间 的 距离 。 
ToF 原 理 和 激光 传 感 莫 十 分 相似 ， 只 不 过 油光 是 通过 逐 操 扫 手 来 获取 距 


离 ， 而 ToF 相 机 则 可 以 获得 整个 图 像 的 像 系 深度 ， 这 也 正定 RGB-D 相 机 
的 特点 。 所 以 ， 如 果 你 把 一 个 RGB-D 相 机 拆 开 ， 通 常会 发 现 除了 普通 的 
摄像 头 忆 外， 至 少 会 有 一 个 肥 射 项 和 一 个 接收 筑 。 


在 测量 深度 之 后 ，RGB-D 相 机 通常 按照 生产 时 的 各 相机 摆 放 位 置 ， 
自己 完成 深度 与 彩色 图 像素 之 间 的 配对 ， 输 出 一 一 对 应 的 彩色 图 和 深度 
图 。 我 们 可 以 在 同一 个 图 像 位 置 ， 读 取 到 色彩 信息 和 距离 信息 ， 计 算 像 
素 的 3D 相 机 坐标 ， 和 生成 点 云 〈(Point Cloud) 。 对 RGB-D 数 据 ， 既 可 以 在 
图 像 层面 进行 处 理 ， 亦 可 在 点 云层 面 处 理 。 本 讲 的 第 二 个 实验 将 演示 
RGB-D 相 机 的 点 云 构建 过 程 。 


RGB-D 相 机 人 能够 实时 地 测量 每 个 像 系 点 的 距离 。 但 是 ， 由 于 这 种 发 
射 -接收 的 测量 方式 ， 其 使 用 范围 比较 受 限 。 用 红外 光 进 行 深 度 值 测量 
的 RGB-D 相 机 ， 容 易 受 到 日 光 或 其 他 传感器 发 射 的 红外 光 王 扰 ， 因 此 不 
能 在 室外 使 用 ， 同 时 使 用 多 个 时 也 会 相互 干扰 。 对 于 透射 材质 的 物体 ， 
因为 接收 不 到 反射 光 ， 所 以 无 法 测量 这 些 点 的 位 置 。 此 外 ，RGB-D 相 机 
在 成 本 、 功 耗 方面 ， 都 有 一 些 劳 势 。 


5.2 图 像 


相机 加 上 和 镜头， 把 三 维 世 界 中 的 信息 转换 成 了 一 张 由 像 闵 组 成 的 照 
片 ， 随 后 存储 在 计算 机 中 ， 作 为 后 续 处 理 的 数据 来 源 。 在 数学 中 ， 图 像 
可 以 用 一 个 矩阵 来 搬 述 ， 而 在 计算 机 中 ， 它 们 占据 一 段 连 续 的 磁 禹 或 内 
和 存 空间 ， 可 以 用 二 维 数组 来 表示 。 这 样 一 来 ， 程 序 就 不 必 区 别 它们 处 理 
的 是 一 个 数值 矩阵 ， 还 是 有 实际 意义 的 图 像 了 。 

本 节 ， 我 们 将 介绍 计算 机 图 像 处理 的 一 些 基 本 操作 。 特 别 地 ， 通 过 
OpenCV 中 图 像 数据 的 处 理 ， 理 解 计 算 机 中 处 理 图 像 的 钊 见 步 骤 ， 为 后 
续 章 有 打下 基础 。 


计算 机 中 图 像 的 表示 

我 们 从 最 简单 的 图 像 一 一 灰 度 图 说 起 。 在 一 张 灰 硫 图 中 ， 每 个 像 系 
位 置 (xy ) 对 应 一 个 灰 度 值 T ， 所 以 ， 一 张 宽 度 为 w . mike Ah WEZ, 
数学 上 可 以 记 为 一 个 窍 阵 : 


I(x,y) e R"*^, 
然而 ， 计 算 机 并 不 能 表达 整个 实数 空间 ， 所 以 我 们 只 能 在 东 个 范围 
内 对 图 像 进 行 量化 。 例 如 ， 利 见 的 灰 度 图 中 ， 用 0 一 255 的 整数 《〈 即 一 个 


unsigned char, 145—580 来 表达 网 像 的 灰 度 大 小 。 那 么 ， 一 张 宽 度 为 
6400 ZR. f E 73480 D AR HB AR BE EE n] EA ez 73 





1 | unsigned char image[480] [640]; 


为 什么 这 里 的 二 维 数组 是 480x 64008? 因为 在 程序 中 ， 图 像 以 二 维 
数组 形式 存储 。 它 的 第 一 个 下 标 古 指数 组 的 行 ， 而 第 二 个 下 标 则 是 列 。 
在 图 像 中 ， 效 组 的 行 数 对 应 图 像 的 高 度 ， 而 列 数 对 应 图 像 的 宽度 。 


FHSS RIA ANA. ARAM EHR AA. SUR 
个 像 系 时 ， 和 需要 指明 它 所 处 的 坐标 ， 如 图 5-8 所 示 。 


原点 O x X 5i C, 宽度 ) (RZ E AME 


灰 度 图 : 0-255 8 位 整数 
深度 图 : 0~65535 16 位 整数 
彩色 图 : 多 通道 


(BGR、RGB、RGBA 等 ) 





引 clz13lcla 
| 一 24 位 一 
一 幅 图 像 
图 5-8 ”图 像 坐标 示意 图 。 
图 5-8 左 边 显示 了 传统 像素 坐标 系 的 定义 方式 。 像 系 坐 标 系 原点 位 


于 图 像 的 左上 角 ， 针 轴 辣 右 ，Y 了 轴 间 下 (也 束 古 前 面 所 说 的 uv 坐标 ) 。 
如 果 它 还 有 第 三 个 轴 一 一 Z” 轴 ， 奢 么 根据 石 手法 则 ，Z ， 轴 应 该 是 同 前 
的 。 这 种 定义 方式 是 与 相机 坐标 系 一 任 的 。 我 们 平时 说 的 图 像 的 攻 硫 或 
列 数 ， 对 应 看 X AH; 而 图 像 的 行 数 或 蜗 度 ， 则 对 应 看 它 的 Y 轴 。 


根据 这 种 定义 方式 ， 如 果 我 们 讨论 一 个 位 于 x;yy ”处 的 像素 ， 那 么 它 
在 程序 中 的 访问 方式 应 议 古 


1 | unsigned char pixel = image[y] [x]; 


EY DAKE (xy ) 的 读数 。 请 注意 这 里 的 x My 的 顺序 。 虽 然 我 
们 不 厌 其 烦 地 讨论 坐标 系 的 问题 ， 但 是 像 这 种 下 标 顺 序 的 错误 ， 仍 会 是 
新 手 在 调试 过 程 中 经 党 储 到 的 ， 且 具有 一 定 隐蔽 性 的 错误 之 一 。 如 果 你 
在 写 程 序 时 不 惯 调换 了 x,y 的 坐标 ， 编 诺 器 无 法 提供 任何 信息 ， 而 你 所 
能 看 到 的 只 是 程序 运行 中 的 一 个 越界 错误 而 已 。 

一 个 像素 的 灰 度 可 以 用 8 位 整数 记录 ， 也 就 是 一 个 0 一 255 的 值 。 当 
我 们 要 记录 的 信息 更 多 时 ， 一 个 字 节 仆人 了 束 不 够 了 了。 例如 ， 在 RGB-D 相 
机 的 深度 图 中 ， 记 录 了 各 个 像素 与 相机 之 间 的 距离 。 这 个 距离 通 稼 是 以 
训 米 为 单位 ， 而 RGB-D 相 机 的 量程 通 浓 在 十 几米 左右 ， 超 过 了 255。 这 
时 ， 人 人们 会 采用 16 位 整数 〈C++ 中 的 unsigned short) 来 记录 深度 图 的 售 
轧 ， 也 就 是 位 于 0 一 65536 的 值 。 换 算 成 米 的 话 ， 最 大 可 以 表示 65 米 ， 足 
a ERGB-DAIBBUSH] f « 


彩色 图 像 的 表示 则 需要 通道 (channel) 的 概念 。 在 计算 机 中 ， 我 们 
用 红色 、 绿色 和 览 色 这 三 种 闫 色 的 组 合 来 表达 任意 一 各 色彩。 于 是 对 于 
每 一 个 像素 ， 束 要 记录 其 R、G、B 三 个 数值 ， 每 一 个 数值 就 称 为 一 个 通 
道 。 例 如 ， 最 和 常见 的 彩色 图 像 有 三 个 通才， 每 个 通道 都 由 8 位 整数 表 
S。 在 这 种 规定 下 ， 一 个 像 聚 占据 24 位 空间 。 


通道 的 数量 、 顺 序 都 是 可 以 目 由 定义 的 。 在 OpenCV 的 彩色 图 像 
中 ， 通 道 的 默认 顺序 是 B、G、R。 也 就 是 说 ， 当 我 们 得 到 一 个 24 位 的 像 
素 时 ， 前 8 位 表示 蓝 色 数值 ， 中 间 8 位 为 绿色 ， 最 后 8 位 为 红色 。 同 理 ， 
亦 可 使 用 R、G、B 的 顺序 表示 一 个 彩色 图 。 如 果 还 想 表 达 图 像 的 透明 
度 ， 就 使 用 、G、B、A 四 个 通道 


ir 


53 实践: 图像 的 存 取 与 访问 


下 面 通 过 一 个 演示 程序 来 理解 ， 在 OpenCV 中 图 像 古 如 何 存 取 ， 而 
我 们 义 是 如 何 访 问 其 中 的 像 系 的 。 


5.3.1 Z3 OpenCV 


OpenCV! PEE [f AGEBAT AREE IE. eevee UBER 
的 图 像 处 理 算法 库 。 本 书 也 使 用 OpenCV 做 基本 的 图 像 处 理 。 在 使 用 之 
前 ， 建 议 读者 从 源 代码 安装 它 。 在 Ubuntu 下 ， 有 从 源 代 码 安 装 ” 和 只 安 
As Fe MAF 两 种 方式 可 以 选择 : 

1. 从 源 代 人 码 安 装 ， 是 指 从 OpenCV 网 站 下 载 所 有 的 OpenCV 源 代码 ， 
并 在 机 磊 上 编译 安装 ， 以 便 使 用 。 好 处 是 可 以 选择 的 版 本 比较 让 定 ， 而 
是 能 看 到 源 代 人 码 ， 不 过 需要 人 花费 一 些 编译 时 间 。 

2. 只 安装 库 文件 ， 是 指 通 过 Ubuntu 来 安装 由 Ubuntu 社 区 人 员 已 经 编 
译 好 的 库 文 件 ， 这 样 就 无 须 重 新 编译 一 授 。 

由 于 我 们 使 用 较 新 版 本 的 OpenCV， 所 以 必须 从 源 代码 来 安装 它 。 
一 来 ， 可 以 调整 一 些 编译 选项 ， 匹 配 编程 环境 (例如 ， 需 不 需要 GPU 加 
速 等 ) ; 再 者 ， 源 代码 安装 可 以 使 用 一 些 额 外 的 功能 。OpenCV 目 前 维 
护 了 两 个 主要 版 本 ， 分 为 OpenCV 2.4 系 列 和 OpenCV 3 系列 。 本 书 使 用 
OpenCV 3 系列 。 


由 于 OpenCV 工 程 比较 六 ， 残 不 放 在 本 书 的 3rdparty 下 了 。 请 读者 从 
http:Wopencv.org/downloads.html 下 载 ， 选 择 OpenCV for Linux 4s B[ n] . 
你 会 获得 一 个 像 opencv-3.1.0.z 讯 这 样 的 压缩 包 。 将 它 解 压 到 任意 目 孙 
下 ， 我 们 发 现 OpenCV 亦 是 一 个 cmake 工 程 。 


TES VEZ BU. FEHR ZCROpenCV If] HK RT : 


1 | sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4 





-dev libjasper-dev libopenexr-dev libtbb-dev 


事实 上 ，OpenCV 的 依赖 项 很 多 ， 缺 少 东 些 编 诺 项 会 影响 它 的 部 分 
功能 《不 过 我 们 也 不 会 用 到 所 有 功能 ) 。OpenCV 会 在 cmake 阶 段 检 查 依 
赖 项 是 否 会 安装 ， 并 调整 日 己 的 功能 。 如 果 你 的 电脑 上 有 GPU 并 且 和 安装 
了 相关 依赖 项 ，OpenCV 也 会 把 GPU 加 速 打开 。 不 过 对 于 本 书 ， 上 面 那 
LE PRM EWS S o 


随后 的 编译 安装 和 普通 的 cmake 工 程 一 样 ， 请 在 make 之 后 ， 调 用 
sudo make install 将 OpenCV 安 北 到 你 的 机 器 上 【而 不 是 仅仅 编 详 它 )。 
人 钢 机 幽 配置， 这 个 编译 过 程 大 概 和 需要 二 十 分 钟 到 一 个 小 时 不 等 。 如 果 你 
的 CPU 比 较 强 ， 可 以 使 用 “make-j4” 这 样 的 命令 ， 调 用 多 个 线程 进行 编译 

(-j 后 面 的 参数 束 是 使 用 的 线程 数量 ) . ZR, Open VRNE 
在 /usr/local 目 录 下 。 你 可 以 去 寻找 OpenCV 尖 文件 与 库 文 件 的 安装 位 
置 ， 看 看 它们 都 在 哪里 。 为 外 ， 如 果 之 前 已 经 安 站 了 OpenCV 2 系列 ， 
那么 建议 你 把 OpenCV 3 安装 到 别 的 地 方 ( 想 想 这 应 该 如 何 操 作 〉。 


5.3.2 ”操作 OpenCV 图 像 


接 下 来 通过 一 个 例 程 熟悉 一 下 OpenCV 对 图 像 的 操作 。 
由 slambook/ch5/imageBasics.cpp 


#include <iostream> 
#include <chrono> 


using namespace std; 


#include <opencv2/core/core.hpp> 


#include <opencv2/highgui/highgui .hpp> 


int main ( int argc, char** argv ) 
{ 
// 读 取 argv[1] 指定 的 图 像 
cv::Mat image; 
image = cv::imread ( argv[1] ); // cu::imread 函数 读 取 指 定 路 径 下 的 图 像 
// 判断 图 像 文件 是 否 正确 读 取 
if ( image.data == nullptr ) // 数据 不 存在 ， 可 能 是 文件 不 存在 
{ 
cerr<<" X fF " «X«argv [1] <<" ^f 存在 ."<<endl ; 


return 0; 


// 文件 顺利 读 取 ， 首 先 输出 一 些 基本 信息 
cout<<" 图 像 宽 为 "<<image.cols<<", 高 为 "<<image.rows<<", 通 道 数 为 "<<image.channels 
()<<endl; 
cv::imshow ( "image", image ); // 用 cv::imshow 显示 图 像 
cv::waitKey ( 0 ); // 暂停 程序 ， 等 待 一 个 按键 输入 
// 判断 image 的 类 型 
if ( image.type() != CV_8UC1 && image.type() != CV_8UC3 ) 
// 图 像 类 型 不 符合 要 求 
cout<<" 请 输入 一 张 彩 色 图 或 灰 度 图 ."<<endl ; 
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68 


69 


return O0; 


// 遍历 图 像 ， 请 注意 以 下 遍历 方式 亦 可 使 用 于 随机 访问 
// 使 用 std::chrono 来 给 算法 计时 
chrono::steady clock::time point t1 = chrono::steady clock::now(); 


for ( size t y=0; y<image.rows; y++ ) 


1 
for ( size t x-0; x<image.cols; x** ) 
1 
// 访问 位 于 z,y 处 的 像素 
// 用 cv::Mat::ptr 获得 图 像 的 行 指针 
unsigned char* row ptr = image.ptr<unsigned char» ( y ); // rou ptr X 
第 y 行 的 头 指针 
unsigned char* data ptr = &row ptr[ x*image.channels() ]; // data ptr 
指向 待 访问 的 像素 数据 
// 输出 该 像素 的 每 个 通道 ， 如 果 是 灰 度 图 就 只 有 一 个 通道 
for ( int c = 0; c != image.channels(); c++ ) 
1 
unsigned char data = data ptr[c]; // data 为 I(a,y) 第 c 个 通道 的 值 
) 
} 
} 


chrono::steady_clock::time_point t2 = chrono::steady_clock: :now() ; 
chrono::duration<double> time used = chrono: :duration_cast<chrono: :duration< 
double>>( t2-t1 ); 

cout<<1" 遍 历 图 像 用 时 : "««time used.count()««" $/. "««endl; 


// 关于 cv::Mat HFN 

// 直接 赋值 并 不 会 复制 数据 

cv::Mat image_another = image; 

// 修改 image_another 会 导致 image 发 生变 化 

image another ( cv::Rect ( 0,0,100,100 ) ).setTo (0); // 将 左上 角 100x100 的 
块 置 零 

cv::imshow ( "image", image ); 


cv::waitKey ( O ); 


// 使 用 clone Hak KL d] KE 

cv::Mat image clone = image.clone(); 

image clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 ); 
cv::imshow ( "image", image ); 

cv::imshow ( "image clone", image clone ); 


cv::waitKey ( O ); 


// 其 他 图 像 操 作 请 参见 OpenCV 官方 文档 ， 查 询 每 个 函数 的 调用 方法 


70 cv::destroyAllWindows(); 


71 return O0; 


在 该 例 程 中 ， 我 们 误 示 了 如 下 几 个 操作 : BARR, Ab. BAN 
历 、 复 制 、 赋 值 等 。 大 部 分 的 注解 已 写 在 代码 里 面 。 编 详 访 程序 时 ， 你 
需要 在 CMakeLists.txt 中 这 加 OpenCV 的 头 文 件 ， 然 后 把 程序 链接 到 库 文 
件 上 。 同 时 ， 由 于 使 用 了 C++11 标 准 〈 如 nullptr 和 chrono) ， 还 需要 设 
置 一 下 编 详 其 : 


# 添加 C++ 11 标准 支持 
set( CMAKE CXX FLAGS "-std=ct+11" ) 


# 寻找 OpenCV Æ 

find_package( OpenCV REQUIRED ) 

# 添加 头 文件 

include directories( ${OpenCV_INCLUDE_DIRS} ) 


add executable( imageBasics imageBasics.cpp ) 
# 链接 OpenCV Æ 
target link libraries( imageBasics ${OpenCV_LIBS} ) 





KTA, RIIA Ha La LB]: 


1. 程 序 从 argv[1]， 也 残 是 命令 行 的 第 一 个 参数 中 该 取 图 像 位 置 。 我 
们 为 读者 准备 了 一 张 几 像 (ubuntu.png， 一 张 Ubuntu 的 壁纸 ， 和 希望 你 喜 
XK) 供 测 试 使 用 。 因 此， 编译 之 后 ， 使 用 如 下 命令 调用 此 程序 : 如果 在 
Kdevelop 中 调用 此 程序 ， 请 务必 确保 把 参数 同时 给 它 。 这 可 以 在 局 动 项 
中 配置 。 


1 | build/imageBasics ubuntu. png 


2 FEFPAIO~17%7, 1 Fl ev::imread el ZEE AR, SEF SEAS 
信息 显示 出 来 。 


3. 在 32 一 52 行 ， 通 历 了 图 像 中 的 所 有 像 际 ， 并 计算 了 整个 循环 所 用 
的 时 | 则 。 请 注意 像 系 的 退 历 方式 并 不 是 唯一 的 ， 而 且 例 程 给 出 的 方式 也 
ANE LH. OpenCVsetit Sik Ras, PREY CIA as le RY 
RA. BMGT, cv::Mat::datafe tt f fala ES EST SUBE. PRAT 
接 通 过 访 指 针 目 行 计 算 偏 移 量 ， 然 后 得 到 像素 的 实际 内 存 位 置 。 例 程 所 
用 的 方 去 是 为 了 便于 读者 理解 图 像 的 结构 。 在 笔者 的 机 右上 《虚拟 
HO ， 遍 历 这 张 图 像 用 时 大 约 12.74ms。 你 可 以 对 比 一 下 自己 机 器 上 的 
速度 。 不 过 ， 我 们 使 用 的 是 cmake 默 认 的 debug 模 式 ， 如 果 使 用 release 模 
式 会 快 很 多 。 

4.0penCV 提 供 了 许多 对 图 像 进 行 操作 的 函数 ， 我 们 在 此 不 一 一 列 
举 ， 人 否则 本 书 承 会 变 成 OpenCV 操 作 手 册 了 。 例 程 给 出 了 较为 利 见 的 读 
取 、 显 示 操 作 ， 以 及 复制 图 像 中 可 能 陷入 的 深 揽 贝 误 区 。 在 编程 过 程 
中 ， 读 者 还 会 页 到 图 像 的 旋转 、 插 值 等 操作 ， 这 时 你 应 该 目 行 查 阅 疯 数 
对 应 的 区 档 ， 以 了 解 它们 的 原理 与 使 用 方式 。 


应 该 指出 ，OpenCV 并 不 是 唯一 的 图 像 库 ， 它 只 是 许多 图 像 库 里 使 
用 苑 围 较 广泛 的 一 个 。 不 过 ， 多 数 图 像 库 对 图 像 的 表达 是 大 同 小 卉 的 。 
我 们 希望 谈 者 了 解 了 OpenCV 对 图 像 的 表示 后 ， 能 够 理解 其 他 库 中 国 像 
的 表达 ， 从 而 在 需要 数据 格式 时 能 够 自己 处 理 。 

男 外 ， 由 于 cv::Mat 亦 是 矩阵 类 ， 际 了 表示 图 像 之 外 ， 我 们 也 可 以 用 
它 来 存储 位 姿 等 算 阵 数据 。 只 是 一 般 认 为 ，Eigen 对 于 固定 大 小 的 阜 阵 
使 用 起 来 效率 更 高 一 些 。 


最 后 ， 我 们 来 练习 一 下 相机 内 外 参 的 使 用 方法 。 本 贡 程 序 提供 了 5 
张 RGB-D 图 像 ， 并 且 知 道 每 个 图 像 的 内 参 和 外 参 。 根 据 RGB-D 图 像 和 
相机 内 参 ， 我 们 可 以 计算 任何 一 个 像素 在 相机 坐标 系 下 的 位 置 。 同 时 ， 
根据 相机 位 次 ， 又 能 计算 这 些 像素 在 世界 坐标 系 下 的 位 置 。 如 采 把 所 有 
像 系 的 空间 坐标 部 求 出 来 ， 相 当 于 构建 一 张 类 似 于 地 图 的 东西 。 现 在 我 
TRA — F -o 

我 们 准备 了 5 对 图 像 ， 位 于 slambook/ch5/joinMap 中 。 在 color/ 下 有 
1.png 到 5.png 共 5 张 RGB 图 ， 而 在 depth/ 下 有 5 张 对 应 的 深度 图 。 同 时 ， 
pose.txt 文 件 给 出 了 5 张 图像 的 相机 位 姿 《 以 T ,形式 ) 。 位 姿 记 录 的 形式 
Fe PFS [A] Be I ee FE VY CŽ: 

IZ, Y, Z, qx, dy dz: qwl, 
其 中 q, 是 四 元 数 的 实 部 。 例 如 ， 第 一 对 图 的 外 参 为 : 


| 一 0.228993, 0.00645704, 0.0287837, —0.0004327, —0.113131, —0.0326832, 0.993042]. 


下 面 我 们 写 一 段 程序 ， 完 成 两 件 事 : (1). 根 据 内 参 计 算 一 对 RGB-D 
图 像 对 应 的 点 云 ; (2). 根 据 各 张 图 的 相机 位 姿 《〈 也 束 是 外 参 ) ， 把 点 云 
加 起 来 ， 组 成 地 图 。 


本 书 的 点 云 库 使 用 PCL (Point Cloud Library) " . PCLHY) zz3& Lbs 


容易 ， 执 行 以 下 命令 即 可 ， 安 装 完成 后 ，PCL 的 头 文件 将 安装 
在 /usr/include/pcl-1.7 下 ， 库 文件 位 于 /sr/lib/ 下。 







1 | sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl 
2 | sudo apt-get update 
3 | sudo apt-get install libpcl-all 


现在 编写 拼接 部 分 的 程序 : 


kò slambook/ch5/ joinMap/joinMap.cpp 


#include 


#include 


<iostream> 


<fstream> 


using namespace std; 


#include 
#include 
#include 
#include 
#include 
#include 


#include 


<opencv2/core/core.hpp> 
<opencv2/highgui/highgui.hpp> 
<Eigen/Geometry> 

<boost/format.hpp> // for formating strings 
<pcl/point_types.h> 

<pcl/io/pcd_io.h> 


<pcl/visualization/pcl_visualizer.h> 


int main( int argc, char** argv ) 


1 
vector<cv::Mat> colorImgs, depthImgs; // 彩色 图 和 深度 图 
vector<Eigen: :Isometry3d> poses; // 相机 位 姿 
ifstream fin("./pose.txt"); 
if (!fin) 
1 


for 


cerr<<" 请 在 有 pose.txt 的 目录 下 运行 此 程序 "<<end]l; 


return 1; 


( int i=0; i<5; i++ ) 


boost::format fmt( "./%s/%d.%s" ); // 图 像 文件 格式 


colorImgs.push back( cv::imread( (fmt/"color"4(i*i)/Á"png").str() )); 
depthImgs.push_back( cv::imread( (fmt/"depth"%(i+1)4"pgm").str(), -1 )); // 


使 用 -1 读 取 原始 图 像 


double data[7] = {0}; 
for ( auto& d:data ) 
fin>>d; 


Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); 


Eigen::Isometry3d T(q); 


T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] )); 


poses.push back( T ); 


// 计算 点 云 并 拼接 
// 相机 内 参 
double cx = 325.5; 


double cy 
double fx 
double fy 


253.5; 
518.0; 
519.0; 


double depthScale - 1000.0; 


cout<<" 正 在 将 图 像 转换 为 点 云 ..."<<endl; 

// 定义 点 云 使 用 的 格式 : 这 里 用 的 是 XYZRGB 
typedef pcl::PointXYZRGB PointT; 

typedef pcl::PointCloud<PointT> PointCloud; 


// 新 建 一 个 点 云 
PointCloud: :Ptr pointCloud( new PointCloud ); 


for ( int i=0; i<5; i++ ) 


{ 


cout<<1" 转 挽 图像 中 : "<<it+1<<endl; 
cv::Mat color = colorImgs[i]; 


cv::Mat depth = depthImgs[il; 


Eigen::Isometry3d T = poses[il; 


for ( int v=0; v<color.rows; V++ ) 


for ( int u=0; u<color.cols; ut+ ) 


1 


unsigned int d = depth.ptr«unsigned short» ( v )[u]; // 深度 值 
if ( d==0 ) continue; // 为 0 表示 没有 测量 到 
Eigen::Vector3d point; 


point[2] = double(d)/depthScale; 
point [0] = (u-cx)*point[2]/fx; 
point[1] = (v-cy)*point[2]/fy; 


Eigen::Vector3d pointWorld = T*point; 


PointT p ; 
x = pointWorld[0]; 
= pointWorld[1]; 
- pointWorld[2]; 


color.data[ v*color.step*u*color.channels() ]; 


= color.data[ v*color.step*u*color.channels()*1 ]; 


'O "OQ Ve Ve Y 
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.r = color.data[ v*color.steptu*color.channels()+2 ]; 


pointCloud->points.push_back( p ); 


pointCloud-»is dense = false; 
cout««" 5 A # 7] "<<pointCloud->size()<<"4% ."««endl; 


pcl::io::savePCDFileBinary("map.pcd", *pointCloud ); 


return O0; 


— A WH: 

1.14 一 37 行 ， 谈 取 彩 色 和 深度 图 像 对 及 位 姿 信息 ， 并 把 位 姿 从 四 元 
数 与 平移 同 量 转换 为 变换 窍 阵 。 注 意 程 序 里 使 用 了 boost::format 进 行 字 
FTE NAT EL © 

2.65~7847: 计算 位 于 (wy )、 深 度 为 d 的 保 系 在 相机 坐标 系 下 的 位 
置 ， 并 根据 外 参 把 它们 变换 到 世界 坐标 。 我 们 知道 ， 相 机 坐标 p。 到 像素 
坐标 (u,v,q ) 的 关系 为 


d Y = K pe. (5.17) 


肥 推 p. WIESJE BE. Wp, =[xyz]， 那 么 : 





2d 
人 一 cz 
T E 
y= — I, 
fy 


3. 为 了 编译 此 程序 ， 我 们 震 3 个 库 : Eigen、OpenCV 和 PCL。 因 此 主 
程序 的 CMake-Lists.txt 应 该 如 下 : 


1 | # opencv 
2 |find_package( OpenCV REQUIRED ) 
3 | include directories( ${OpenCV_INCLUDE_DIRS} ) 


5 |# eigen 


6 | include directories( "/usr/include/eigen3/" ) 


s | # pcl 

9 |find package( PCL REQUIRED COMPONENT common io ) 
10 | include directories( $1PCL INCLUDE DIRS) ) 

1 |add definitions( ${PCL_DEFINITIONS} ) 


i3 | add, executable( joinMap joinMap.cpp ) 
i4 | target link libraries( joinMap ${OpenCV_LIBS} $(PCL, LIBRARIES) ) 





最 后 ， 我 们 把 生成 的 点 云 以 PCD 格 式 和 存储 在 map.pcd 中 。 用 PCL 提 供 
的 可 视 化 程序 打开 这 个 文件 : 


1 |pcl_viewer map.pcd 
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图 5-9 ”查看 拼合 的 点 云 地 图 。 





在 这 个 例 程 中 ， 我 们 使 用 相机 内 参 和 外 参 来 计算 像 系 在 世界 坐标 系 
中 的 位 置 ， 并 把 它们 合并 成 一 个 点 云 。 这 是 一 个 综合 性 的 示例 ， 请 读者 


仔细 体会 并 掌握 其 内 容 。 

2] i 

寻找 一 部 相机 《你 的 手机 或 笔记 本 的 摄像 头 即 可 ) ， 标 定 它 的 

45. 你 可 能 会 用 到 标定 板 ， 或 者 目 己 打印 一 张 标 定 用 的 棋盘 格 。 

2. 叙 述 相机 和 内参 的 物理 意义 。 如 采 一 部 相机 的 分 辩 率 变 为 原来 的 网 
倍 而 其 他 地 方 不 变 ， 它 的 内 参 如 何 变化 ? 

3. 搜 索 特 殊 相 机 “《 鱼 眼 或 全 景 相 机 ) 的 标定 方法 。 它 们 与 普通 的 针 
筷 模 型 有 何不 同 

4. 调 研 全 局 快门 相机 Cglobal shutter) MEA Ar JAAN. Crolling 
shutter) KJ. EIZESLAMY A PER? 

5.RGB-D 相 机 是 如 何 标定 的 ? 以 Kinect 为 例 ， 需 要 标定 哪些 参数 ”? 
(Z5 https://github.com/code-iai/iai kinect2. ) 


ez AAA) 


6. 除 了 示例 程序 演示 的 过 历 图 像 的 方式 ， 你 还 能 举 出 哪些 珊 历 图 像 


f) 1? 
7.* 阅 读 OpenCV 官 方 教程 ， 学 习 它 的 基本 用 法 。 





[1] 或 图 像 坐标 系 ， 见 本 讲 第 2 市 。 
[2] 是 的 ， 它 不 再 直 了 ， 而 是 变 成 弯 的 。 如 果 往 里 弯 ， 称 为 桶 形 失 真 ， 往 外 弯 则 是 枕 形 失真 。 


[3] 注意 到 Z 可 能 小 于 1， 说 明 该 氮 位 于 归 一 化 平面 后 面 ， 它 可 能 不 会 在 相机 平面 上 成 像 ， 实 点 
当中 要 检查 一 次 。 


[4] 那样 的 话 外 观 会 有 些 奇 特 。 
[5] 读者 可 以 目 己 用 眼睛 模拟 一 下 。 


[6] 官方 主页 : http://opencv.org。 











[7] 官网 : http://pointclouds.org/。 


Xr 


[8] 在 Ubuntu 16.04 下 请 通过 apt-get 安 装 ， 想 想 这 该 怎么 做 。 


第 6 讲 ” 非 线性 优化 


EA tp 
1. EES ine) ETA URBE] A 


2. 理 解 珊 斯 牛顿 法 CGauss-Newton) . 4) SC{At— 3 SRR IE 
(Levenburg-MarquadO = FATE - 


3. 学 习 Ceres 库 和 g2o 库 的 基本 使 用 方法 。 


在 前 面 几 讲 ， 我 们 介绍 了 经 典 SLAM 模 型 的 运动 方程 和 观测 方程 。 
现在 我 们 已 经 知 站 ， 方 程 中 的 位 姿 可 以 由 变换 窍 阵 来 朱 述 ， 然 后 用 李 代 
数 进行 优化 。 观 测 方 程 由 相机 成 像 模 型 给 出 ， 其 中 内 参 古 随 相 机 固定 
的 ， 而 外 参 则 是 相机 的 位 姿 。 于 是， 我 们 已 经 弄 消 了 经 典 SLAM 模 型 在 
视觉 情 况 下 的 有 共 体 表达 。 

然而 ， 由 于 噪声 的 存在 ， 运 动 方程 和 观 训 方 程 的 等 式 必 定 不 是 精确 
成 立 的 。 尽 过 相机 可 以 非 癌 好 地 符合 针 孔 模型 ， 但 遗 慨 的 十， 我 们 得 到 
的 数据 通 铅 是 党 各 种 未 知 噪声 影响 的 。 即 使 我 们 有 高 精度 的 相机 ， 运 动 
FI REAM TT PETER BEAM ATA, MER BE DAT BT TE, 
不 如 来 讨论 如 何在 有 噪声 的 数据 中 进行 准确 的 状态 佑 计 。 

大 多 数 现代 视觉 SLAM 算 法 都 不 需要 那么 高 成 本 的 传 感 硕 ， 甚 至 也 
不 需要 那么 昂 贯 的 处 理 吉 来 计算 这 些 数据 ， 这 全 和 是 算法 的 功 歼 。 由 于 在 
SLAM 问 题 中 ， 同 一 个 后 往往 会 被 一 部 相机 在 个 同 的 时 间 内 多 人 炊 观测 ， 
同一 部 相机 在 每 个 时 刻 观 测 到 的 点 也 不 止 一 个 。 这 些 因 系 交织 在 一 起 ， 
使 我 们 拥有 了 更 多 的 约束 ， 最 终 能 够 较 好 地 从 吕 声 数据 中 恢复 出 我 们 十 
要 的 东西 。 本 市 束 将 介绍 如 何 明 过 优化 处 理 噪声 数据 ， 并 且 由 这 些 表 层 
逐渐 深入 到 图 优化 本 质 ， 给 出 图 优化 的 解决 握 法 初步 介绍 并 且 提 供 训练 
实例 。 
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6.1 ”状态 估计 问题 


6.1.1 最 大 后 验 与 最 大 似 然 


接 看 前 面 几 讲 的 内 容 ， 我 们 回顾 一 下 第 2 讲 讨论 的 经 典 SLAM 柑 
型 。 它 由 一 个 状态 方程 和 一 个 运动 方程 构成 ， 如 式 (2.5) 所 示 : 


(6.1) 


Lk = f (Lk-1, Uk) + We 
Zkj = h(yj, tx) + vx; 


通过 第 4 讲 的 知识 ， 我 们 了 解 到 这 里 的 %， 力 古 相机 的 位 姿 。 我 们 可 


以 使 用 变换 矩阵 或 李 代 数 表 示 它 。 至 于 观测 方程 ， 第 5 讲 已 经 说 明 ， 即 
针 孔 相机 模型 。 为 了 让 读者 对 它们 有 更 深 的 印象 ， 我 们 不 妨 讨论 一 下 其 
具体 参数 化 形式 。 首 先 ， 位 姿 变量 x 可 以 由 T 或 exp(s* ) 表 达 ， 二 者 是 等 
价 的 。 由 于 运动 方程 在 视觉 SLAM 中 没有 特殊 性 ， 我 们 和 暂且 不 讨论 ， 而 
主要 讨论 观测 方程 。 假 设 在 x 处 对 路 标 y, 进行 了 一 次 观测 ， NÉ 
上 的 像素 位 置 z, ， 那 么 ， 观 测 方程 可 以 表示 成 


825.5 = K ep") yj (6.2) 


根据 上 一 讲 的 内 容 ， 读 者 应 该 知道 ， 这 里 K 为 相机 内 参 ，s 为 像素 
扩 的 距离 。 同 时 ， 这 里 的 z, My, 都 必须 以 齐 次 坐标 来 描述 ， 且 中 间 有 一 
次 齐 次 到 非 齐 次 的 转换 。 如 果 你 还 不 熟悉 这 个 过 程 ， 请 回 到 上 一 讲 再 仔 
ZH [ri] TEE o 
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中 ， 我 们 通 第 假设 两 个 噪声 项 w, ,v 满足 零 均 值 的 高 斯 分 布 : 


Wk ~ N (0, Ri.) , Uk ^ N (0, Q;. ;) . (6.3) 


在 这 些 噪声 的 影响 下 ， 我 们 布 望 通过 市 噪声 的 数据 z 和 u 推 其 位 姿 x 
和 地 图 y 《以 及 它们 的 概率 分 布 ) ， 这 构成 了 一 个 状态 估计 问题 。 由 于 
在 SLAM 过 程 中 ， 这 些 数据 是 随时 间 逐 湖 到 来 的 ， 所 以 在 历史 上 很 长 一 
BITEN, PCa re Aisa, EHEN HERR ár (EKF) K 
MEE. FREIREN OAW AAS itx o A AARS DU] AP 
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视觉 SLAM 的 主流 方法 。 因 此 ， 本 书 重 点 介绍 以 非 线性 优化 为 主 的 优化 
方法 ， 对 卡尔 受 泪 波 夯 则 留 到 第 10 讲 再 进行 讨论 。 本 讲 将 介绍 非 线性 优 
化 的 基本 知识 ， 然 后 在 第 10 讲 、 第 11 讲 中 对 它们 进行 更 深入 的 分 析 。 


首 乞 ， 从 概率 学 角度 来 看 一 下 我 们 正在 讨论 什么 问题 。 在 非 线性 优 
化 中 ， 把 所 有 每 信 计 的 变量 放 在 一 个 “状态 变量 ”中 : 


现在 ,我们 说 ， 对 机 右 人 状态 的 估计， 束 古 已 知 输入 数据 u 和 观测 
数据 z 的 条 件 下 ， 求 计算 状态 x 的 条 件 概率 分 布 : 


P(z|z, u). (6.4) 


类 似 于 x ， 这 里 u Mz 也 是 对 所 有 数据 的 统称 。 特 别 地 ， 当 没有 测量 
运动 的 传 感 占 ， 而 只 有 一 张 张 的 图 像 时 ， 即 只 考虑 观测 方程 种 来 的 数据 
上 时， 相当 于 估计 P (xz ) 的 条 件 概 深 分 布 。 如 末 忽 略图 像 在 时 则 上 的 联 
系 ， 把 它们 看 作 一 堆 彼 此 没有 关系 的 图 片 ， 该 问题 也 称 为 Structure from 
Motion (SfM) ， 即 如 何 从 许多 图 像 中 重建 三 维 空间 结构 P 。 在 这 种 情 
况 下 ，SLAM 可 以 看 作 图 像 具 有 时 间 乞 后 顺序 ， 需 要 实时 求解 一 个 SfM 
问题 。 为 了 估计 状态 变量 的 条 件 分 布 ， 利 用 贝 叶 斯 法 则 ， 有 : 


P (z|x) P (x) 


Pas = P (z) 


oc (epee Cay (6.5) 


D pr VU Ze JUS BR A J Se, AGEN (zix ) 称 为 似 然 » IH 
一 部 分 P (x ) 称 为 先 验 。 直 接 求 后 验 分 布 是 困难 的 ， 但 是 求 一 个 状态 最 
优 人 估计， 使 得 在 该 状态 下 后 验 概率 最 大 化 ( Maximize a Posterior, 


MAP) ， 则 是 可 行 的 : 
x“ wap = argmax P (a2|z) = arg max P(e) Pa) (6.6) 


请 注意 贝 叶 斯 法 则 的 分 母 部 分 与 每 估计 的 状态 x 无 关 ， 因 而 可 以 忽 
略 。 贝 叶 斯 法 则 悍 诉 我 们 ， 求 解 最 大 后 验 概率 相当 于 最 大 化 似 然 和 先 
验 的 乘积 ”。 进 一 步 ， 我 们 当然 也 可 以 说 ， 对 不 起 ， 我 不 知道 机 右 人 位 
次 大 概 在 什么 地 方 ， 此 时 就 没有 了 先 验 。 那 么 ， 可 以 求解 x 的 最 大 似 然 
估计 C Maximize Likelihood Estimation, MLE) : 


w ME = arg max P(z|z). (6.7) 
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6.1.2 ”了 最 小 二 乘 的 引出 

那么 如 何 求 最 大 似 然 佑 计 呢 ? 我们 说 ， 在 融 斯 分 布 的 假设 下 ， 最 大 
似 然 能 够 有 较 简 单 的 形式 。 回 顾 观 测 模型 ， 对 于 某 一 次 观测 : 

Zeg = h(yj, tk) t Ux, 
由 于 我 们 假设 了 噪声 项 w ~N (0,Q )， 所 以 观测 数据 的 条 件 概 认为 
P(zjk|*k, uj) =N (h(yj, £k), Qx.;). 

它 依 然 是 一 个 高 斯 分 布 。 为 了 计算 使 它 最 大 化 的 x y, ， 我 们 往往 使 

用 最 小 化 负 对 数 的 方式 来 求 一 个 高 斯 分 布 的 最 大 似 然 。 


高 斯 分 布 在 负 对 数 下 有 较 好 的 数学 形式 。 考 虑 任意 高 维 融 斯 分 布 x 
~N (p>)， 它 的 概率 密度 函数 展开 形式 为 


l l den 
P (x) = ——— exp | —-(x -u) © (mx-—p)|. (6.8) 
(Qn) det(S) c ) 


对 其 取 负 对 数 ， 则 变 为 
-In (P (a) = Z In ((2x)" det (33) + 5(@ — 4) ^ (æ — n). (6.9) 


对 原 分 布 求 最 大 化 相当 于 对 负 对 数 求 最 小 化 。 在 最 小 化 上 式 的 x 
HI, 98—J3 x AR, MURS. Fe, KRMA REN, 
束 得 到 了 对 状态 的 最 大 似 然 估计 。 代 入 SLAM 的 观测 模型 ， 相 当 于 在 
K: 


x" = arg min (F — h (£k, yj)) Qi. (Zk j — h (£k, y;))) | (6.10) 


RIEM, ZAST ae MERED RÆ) 的 平方 Ce BOS 
SCR) 。 因 此 ， 对 于 所 有 的 运动 和 任意 的 观测 ， 我 们 定义 数据 与 估计 值 
CIB RAE 


Cy k = £y — f (Z& 1, Ur) (6.11) 
€y j,k = “kj =h (2x, Y;) . 


A =ł E —1 
F(a) = Y eL Rz ese Y ep ks pg (612) 
k k j 


并 求 该 误 兰 的 平方 和 : KERE P CN AAS OB ic) 3f fn] 
i (Least Square Problem) 。 我 们 明日 它 的 最 优 解 等 价 于 状态 的 最 大 似 
然 估计。 直观 讲 ， 由 于 噪声 的 存在 ， 妆 我 们 把 估计 的 轨迹 与 地 图 代入 
SLAM 的 运动 、 观 测 方程 中 时 ， 它 们 并 不 会 完美 地 成 立 。 这 时 怎么 办 
We? 我 们 对 状态 的 估计 值 进行 微调 ”， 使 得 整体 的 误 到 下 降 一 些 。 当 然 
这 个 下 降 也 有 限度 ， 它 一 般 会 到 达 一 个 极 小 值 ”。 这 束 是 一 个 典型 非 线 


性 优化 的 过 程 。 

仔细 观察 式 (6.12)， 我 们 发 现 SLAM 中 的 最 小 二 乘 问题 具有 一 些 特定 
的 结构 : 

首先 ， 人 整个 问题 的 目标 函数 由 许多 个 误 甜 的 〈 加 权 的 ) 平方 和 组 
成 。 虽 然 总 体 的 状态 变量 维 数 很 高 ， 但 每 个 误 甜 项 都 是 简单 的 ， 仅 与 一 
两 个 状态 变量 有 关 。 例 如 ， 运 动 误 甜 只 与 X_ , ox, AR, MAIRA R x, 
Y; 有关。 每 个 误 天 项 部 是 一 个 小 规模 的 约束 ， 我 们 之 后 会 谈论 如 何 对 它 
们 进行 线性 近似 ， 最 后 再 把 这 个 误 盈 项 的 小 雅 可 比 矩 阵 块 放 到 整体 的 雅 
可 比 窍 阵 中 。 由 于 这 种 做 法 ， 我 们 称 每 个 误 甜 项 对 应 的 优化 变量 为 参数 
块 ( Parameter Block) . 
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会 具有 一 定 的 稀世 性 《会 在 第 10 讲 详细 讲解 ) ， 便 得 它们 在 大 规模 时 人 
可 求解 。 

其次， 如 本 使 用 李 代 数 表 示 ， 则 该 问题 是 无 约束 ”的 最 小 二 乘 问 
题 。 但 如 采用 旋转 窍 阵 《〈 变 换 和 矩阵 ) FIR Zs, MSS] Ate Fe FE BE A A 
的 约束 《旋转 沧 阵 必须 是 正 交 滤 阵 且 行 列 式 为 1) o BUDA HR oe TET 
化 变 得 更 困难 。 这 体现 了 李 代 数 的 优势 。 

最后， 我们 使 用 了 平方 形式 (二 范 数 ) 度量 误差 ， 它 是 直观 的 ， 
相当 于 欧 氏 空间 中 距离 的 平方 。 但 也 存在 奢 一 些 问题 ， 并 且 不 是 唯一 的 
度量 方式 。 我 们 尔 可 使 用 其 他 的 范 数 构建 优化 问题 。 


现在 ， 我 们 介绍 如 何 求解 这 个 最 小 二 乘 问题 。 本 讲 将 介绍 非 线 性 优 
化 的 基本 知识 ， 特 别 地 ， 针 对 这 样 一 个 通用 的 无 约束 非 线性 最 小 二 乘 
问题 ， 探 讨 它 是 如 何 求解 的 。 在 后 续 几 讲 ， 我 们 会 大 量 使 用 本 讲 的 结 
果 ， 详 细 讨 论 它 在 SLAM 前 端 、 后 端 中 的 应 用 。 


6.2 JERE NZK 


我 们 先 来 考虑 一 个 简单 的 最 小 二 乘 问 题 : 


1 
min 7 ||f (æ). (6.13) 


这 里 目 变 量 xE R” ,了 是 任意 非 线 性 函数 ， 我 们 设 它 有 m Œ: f (x 
JE Rm"。 下 面 讨论 如 何 求 解 这 样 一 个 优化 问题 。 

"Rf 是 个 数学 形式 上 很 简单 的 函数 ， 那 么 问题 也 许可 以 用 解析 形 
式 来 求 。 令 目标 函数 的 导数 为 堆 ， 然 后 求解 x HEE. MARIEK 
ZIEL HF : 


df 
7 = 0. (6.14) 

解 此 方程 ， 了 驶 得 到 了 导数 为 零 处 的 极 值 。 和 它们 可 能 是 极 太 、 极 小 或 
BARI, AEST ERE ITN BU A/D BU Ay. (Ae, STi 
BE AKENET 这 取 次 于 太 FAAEE. FESLAMH, FATEH ERN 
TOR Lii A Tee ALS s JR RAT ESN V6 SES BL 
形式 ， 但 这 不 代表 我 们 就 能 够 顺利 求解 上 式 这 样 一 个 复 末 的 非 线 性 方 
程 。 

对 于 不 方便 直接 求解 的 最 小 二 乘 问 题 ， 我 们 可 以 用 运 代 ”的 方式 ， 
从 一 个 和 初始 值 出 友 ， 不 断 地 更 新 当前 的 优化 变量 ， 使 目标 函数 下 降 。 具 
体 步 又 可 列 写 如 下 : 


1. 给 定 某 个 初始 值 x 。。 

2. 对 于 第 k 次 迭代 ， 寻 找 一 个 增 量 Ax, EIF Ax, UH? ,达到 极 小 
值 。 

3. 石 AX HB), WP IE. 


4.75 Wl, BX, a =x, *ÁX, » 返回 第 2 步 。 


这 让 求解 导 函 数 为 零 的 问题 变 成 了 一 个 不 断 寻 找 梯度 并 下 降 的 过 
程 。 直 到 某 个 时 刻 增 量 非常 小 ， 无 法 再 使 函数 下 降 。 此 时 算法 收敛 ， 目 
标 达 到 了 一 个 极 小 ， 我 们 也 就 完成 了 寻找 极 小 值 的 过 程 。 在 这 个 过 程 
中 ， 我 们 只 要 找到 友人 代 点 的 梯度 方向 即 可 ， 而 无 须 寻 找 全 局 导 函 数 为 零 
的 情况 。 

接 下 来 的 问题 是 ， 增 量 Ax， 如 何 确 定 ? Kk, WRACA I 
了 大 量 精力 探索 增 量 的 求解 方式 。 我 们 将 介绍 两 类 办 法 ， 用 不 同 的 手段 
来 寻找 这 个 增 量 。 目 前 这 两 种 方法 在 视觉 SLAM 的 优化 问题 上 被 广泛 采 
用 ， 大 多 数 优 化 库 都 可 以 使 用 它们 。 


6.2.1  — Br RI Br ES BETA 
He ARTE Ec Ee EMRIN S PE H b PRE PEE AT TP: 
lf (2 + Ax)||2 e || fŒ)? + J (x) Aa + =A" H Aa. (6.15) 
这 里 J 是 lf (xl? 关于 x Ae CHET CREME)» WH 则 是 三 阶 导 数 
(#2 (Hessian) 和 窍 隆 )。 我 们 可 以 选择 你 留 泰勒 展开 的 一 阶 或 二 阶 
项 ， 对 应 的 求解 方法 则 为 一 阶 杨 度 或 二 阶 村 度 法 。 如 来 你 留 一 阶梯 度 ， 
HS ZS x HY PRY 
Aa* = —.J (æ). (6.16) 
TEES LW ask CAE Ts fal A, FR BET a Be I RET IRI WEBI nf» 38 
55 3X4] DS VERLA AEN PARA ， 求 得 最 快 的 下 降 方 式 。 这 种 方 
法 和 被 称 为 最 速 下 降 法 。 
FAA Ti, GARR MR E ABATE TEA 


1 
Ax* = arg min || f (a) || + J (x) Ax + 54v 五 Ar (6.17) 


KAMERAK T Ax FAFS EAS, BASE STA: 


HAz = —J'. (6.18) 


该 方法 义 称 为 牛顿 法 。 我 们 看 到 ， 一 阶 和 二 阶 柳 度 法 都 十 分 十 观 ， 
只 要 把 函数 在 欠 代 点 附近 进行 雄 勒 展开 ， 并 针对 更 新 量 做 最 小 化 即 可 。 
由 于 共和 勒 展开 之 后 函数 变 成 了 多 项 式 ， 所 以 求解 增 量 时 只 需 解 线性 方程 
即 可 ， 如 免 了 直接 求 导 函 数 为 零 这 样 的 非 线 性 方程 的 困难 。 不 过 ， 这 两 
种 方法 也 存在 它们 目 里 的 问题 。 最 速 下 降 法 过 于 信心 ， 容 易 走出 锯齿 路 
线 ， 反 而 增加 了 友 代 次 数 。 而 牛顿 法 则 需要 计算 目标 函数 的 五 ABER, IX 
在 问题 规 柑 较 大 时 非 第 困难 ， 我 们 通常 倾 回 于 如 免 互 的 计算 。 所 以 ， 接 
下 来 我 们 评 细 地 介绍 两 类 更 加 实用 的 方法 : 高 期 牛顿 法 和 列 文 伯 格 一 马 


S VN HA 


6.2.2 mi r^ TE 


融 斯 牛顿 法 是 最 优化 算法 中 最 简单 的 方法 之 一 。 它 的 思想 是 将 三 (x ) 
进行 一 阶 的 泰勒 展开 《请 注意 不 是 目标 函数 Fox) ) : 
f (2 + Az) = f (m) + J (x) Az. (6.19) 


3X HJ (x DAP (x ) 天 于 x 的 导数， 实际 上 有 是 一 个 mxm Ep, Es 
雅 可 比 窍 阵 。 根 据 前 面 的 框架 ， 当 前 的 目标 是 寻找 下 降 天 量 Ax . TES 
(x +Ax Jl ? 达到 最 小 。 为 了 求 Ax ， 我 们 需要 解 一 个 线性 的 最 小 二 乘 问 


jel : 
r _ 2 
An” = arg min g l4 (z) + J (x) Aa||”. (6.20) 


这 个 方程 与 之 前 有 什么 个 一 样 呢 ? 根据 极 值 条 件 ， 将 上 述 目 标 函 数 
对 Ax ” 求 村 ， 并 令 导 数 为 零 。 由 于 这 里 考虑 的 是 Ax ”的 寻 数 《而 不 是 x 
) ， 我 们 最 后 将 得 到 一 个 线性 的 方程 。 为 此 ， 移 展开 目标 函数 的 平方 
项 : 


Sif (x) + J (x) Aal|” = z(f (x) + J (a) Ax)’ (f (x) + J (x) Ax) 


1 
2 
1 2 T T T 

= (If Gr) i +2f (x) J(x)Ax + Ax J(x) J(æ)Ax) l 


ESR Ax 的 导数 ， 并 令 其 为 零 : 
2.J(z) f (x) +2I(x)' J (x) Ax = 0. 
可 以 得 到 如 下 方程 组 : 
J(z) J (x) Az = —J(x)' f (ax). (6.21) 

注意 ， 我 们 要 求解 的 变量 是 Ax ， 因 此 这 是 一 个 线性 方程 组 ， 我 们 
称 它 为 增 量 方程  ， 也 可 以 称 为 高 斯 牛顿 方程 (Gauss Newton 
equation) 或 者 正规 方程 (Normal equation) 。 我 们 把 左边 的 系数 定义 
为 万 ， 右 边 定 义 为 9 254A EXE 


HAz =g. (6.22) 


这 里 把 堪 侧 记 作 五 A AY. EGE RI Us. mu ERIT 
J (EAE HEF brHessian te BEE, MMA f VPELH 的 过 程 。 求 
解 增 量 方程 是 整个 优化 问题 的 核心 所 在 “。 如 末 我 们 能 够 顺利 解 出 该 方 
程 ， 那 么 局 斯 牛 贺 法 的 算法 步骤 可 以 写成 : 

1.25 XE 8] UB THX o 。 

2. 对 于 第 k RIER, SK 2A BU B) RT EGRBEEJ (x, AR ef (x, )。 

3. 求 解 增 量 方程 : H Ax, =g o 

AJ Ax, E, WIE. AM, Sx, a =x, +Ax, ， 返 回 第 2 步 。 


从 算法 步 又 中 可 以 看 到 ， 增 量 方程 的 求解 鼎 据 着 主要 地 位 。 原 则 
上 上 ， 它 要 求 我 们 所 用 的 近似 了 HH 是 阵 是 可 逆 的 《而 且 是 正定 的 ) ， 但 实际 
数据 中 计算 得 到 的 JI 了 却 只 有 半 正 定性 。 也 束 是 说 ， 在 使 用 高 斯 牛顿 法 
IY, Ay REMIT Aare te eek aves Cillcondition) 的 情况 ， 此 时 增生 
Hee ERAS, SPB Uo. SEPA ce, OTE HAR ar 
也 非 病 态 ， 如 果 我 们 求 出 来 的 步 长 Ax AK, He SOIR NY D 
近似 (6.19) 不 够 准确 ， 这 梓 一 来 我 们 甚至 无 法 保证 它 的 迭代 收敛 ， 哪 怕 
征 让 目标 函数 变 得 更 大 都 是 有 可 能 的 。 

尽管 高 斯 牛顿 法 有 这 些 缺 点 ， 但 它 依 然 值 得 我 们 去 和 学习， 因为 在 非 


线性 优化 里 ， 相 当 多 的 算法 都 可 以 归结 为 高 斯 牛顿 法 的 变种 。 这 些 算 法 
者 倍 助 了 高 斯 牛顿 法 的 思想 并 且 通 过 目 己 的 改进 修正 其 缺点 。 例 如 一 些 
线 搜索 方法 (line search method)， 这 类 改进 束 是 加 入 了 一 个 标量 aq ， 在 
确定 了 Ax 后 进一步 找到 a 使 得 lf (x ta Ax ) ? 达到 最 小 ， 而 不 是 像 高 斯 牛 
顿 法 那样 徐 音 地 令 a =1。 

列 文 伯 格 一 瑟 侍 尔 特 方法 在 一 定 程 度 上 修正 了 这 些 问题 ， 一 般 认 为 
它 比 高 斯 牛 玉 法 更 为 健壮 。 尽 管 它 的 收敛 速度 可 能 会 比 高 斯 牛顿 法 更 
乙 ， 被 称 为 阻尼 牛顿 法 (Damped Newton Method) , 但 是 在 SLAM 里 
TRI AN ABC REIN HH - 
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EH T reg rA 9 1 P EFI EA Be RTT R BERETTA EA E 
好 的 近似 效果 ， 上 所 以 我 们 很 目 然 地 想到 应 该 给 Ax 添加 一 个 信赖 区 域 
(Trust Region) ， 不 能 让 它 太 大 而 使 得 近似 个 准确 。 非 线性 优化 中 有 有 
一 系列 这 类 方法 ， 这 类 方法 也 被 称 为 信赖 区 域 方法 (Trust Region 
Method) 。 在 信赖 区 域 里 边 ， 我 们 认为 近似 是 有 效 的 ; 出 了 这 个 区 域 ， 
近似 可 能 会 出 问题 。 

那么 如 何 确定 这 个 信赖 区 域 的 范围 呢 ? 一 个 比较 好 的 方法 和 是 根据 我 
们 的 近似 模型 跟 实 际 函 数 之 间 的 差 弄 来 确定 : IR BIS 
围 尽 可 能 大 ;如果 差 开 大 ， 我 们 束 缩 小 这 个 近似 范围 。 因 此 ， 考 感 使 用 


_ f(x * Ax) — f (x) 
m J (x) Ax ) 


RANT as Ee EIE. p 的 分 子 是 实际 图 数 下 降 的 什 ， 分 母 是 
近似 模型 下 降 的 值 。 如 果 p 接近 于 1， 则 近似 是 好 的 。 如 有 果 p 太 小 ， 说 明 
实际 减 小 的 值 远 少 于 近似 减 小 的 值 ， 则 认为 近似 比较 莽 ， 需 要 缩小 近似 
WH. MZ, UWRp 比较 大 ， 则 说 明 实际 下 降 的 比 预计 的 更 大 ， 我 们 可 
以 放大 近似 范围 。 


于 是 ， 我 们 构建 一 个 改 民 版 的 非 线 性 优化 框 染 ， 访 框 染 会 比 高 斯 牛 
HUA A EU CR: 


(6.23) 


1. 给 定 初 始 值 x, ， 以 及 初始 优化 半径 )。 


. 1 
min 5 |f (wx) + J (ax) Azk| ， s.t.|| DAa,||" € p, (6.24) 
Tk 


这 里 4 是 信赖 区 域 的 半径 ，D 将 在 后 文 说 明 。 

3. 计 算 p 。 

Api. Wüp-2u. 

54 pci. Wp =0. Sp 。 

6. 如 果 p 大 于 有 茶 国 值 ， 则 认为 近似 可 行 。 令 X ,1 =x, AX, 。 


7. 判 新 算法 是 售 收 伊 。 如 不 收敛 则 返回 第 2 步 ， 合 则 结束 。 


这 里 近似 范围 扩大 的 僧 数 和 国信 都 十 经 验 仁 ， 可 以 痊 换 成 别 的 数 
值 。 在 却 (6.24) 中 ， 我 们 把 增 量 限定 于 一 个 半径 为 六 的 球 中 ， 认 为 只 在 
这 个 球 内 才 是 有 效 的 。 市 上 D 之 后 ， 这 个 球 可 以 看 成 一 个 椭 球 。 在 列 文 
但 格 提 出 的 优化 方法 中 ， 把 D 取 成 单位 阵 太 ， 相 当 于 和 卫 接 把 Ax 约束 在 一 
个 球 中 。 随 后 ， 杞 夺 尔 特 提出 将 D 取 成 非 负 数 对 角 阵 一 一 实际 中 通 当 
用 下 J 的 对 角 元 条 平方根， 使 得 在 标 度 小 的 维度 上 约束 施 围 更 大 一 些 。 


不 论 如 何 ， 在 列 文 们 格 一 马 僵 尔 特 优化 中 ， 我 们 都 需要 解 式 (6.24) 
那样 一 个 子 问 题 来 获得 标 度 。 这 个 了 于 问题 古市 不 等 式 约束 的 优化 问题 ， 
我 们 用 拉 格 明日 洋子 将 它 转 化 为 一 个 无 约束 优化 问题 : 


1 入 
min 7| (zi) + J (zi) Azk| + |DAz |. (6.25) 


这 里 人 ”为 拉 格 明日 乘 子 。 关 似 于 高 斯 牛 罚 法 中 的 做 法 ， 把 它 展 开 
后 ， 我 们 发 现 该 问题 的 核心 仍 是 计算 增 量 的 线性 方程 : 


(H + AD! D) Ax = g. (6.26) 


可 以 看 到 ， 增 量 方程 相 比 于 局 斯 牛顿 法 ， 多 了 一 项 MD7 D 。 如 来 考 
虑 它 的 简化 形式 ， 即 D =T， 那 么 相当 于 求解 : 


CH + AI) Ax =g. 


我 们 看 到 ， 当 参数 A 比较 小 时 ， 互 占 主 要 地 位 ， 这 说 明 二 次 近似 模 
型 在 该 范围 内 是 比较 好 的 ， 列 文 们 格 一 乌 伪 尔 特 方 法 更 接近 于 高 斯 牛顿 
YE. AA, “SA 比较 大 时 ，XI 占据 主要 地 位 ， 列 文 伯 格 一 马 夸 尔 特 
方法 更 接近 于 一 阶梯 上 度 下 降 法 〈( 即 最 速 下 降 ) ， 这 说 明 附 近 的 二 次 近似 
不 够 好 。 列 文 伯 格 一 马 夸 尔 特 方 法 的 求解 方式 ， 可 在 一 定 程度 上 避免 线 
性 方程 组 的 系数 矩阵 的 非 奇 异 和 病态 问题 ， 提 供 更 稳定 、 更 准确 的 增 量 
Ax 。 


在 实际 中 ， 还 存在 许多 其 他 的 方式 来 求解 函数 的 增 量 ， 例 如 Dog- 
Leg 等 方法 。 我 们 在 这 里 所 介绍 的 ， 只 是 最 营 见 而 且 最 基本 的 方式 ， 也 
是 视 冯 SLAM 中 用 得 最 多 的 方式 。 总 而 言 之 ， 非 线性 优化 问题 的 框架 ， 
分 为 Line Search 和 Trust Region 两 类 。Line Search 先 固定 搜索 方向 ， 然 后 
EZI APR AAR, UPOR BREA AD ee EE Ne. M Trust 
Region l 7c EH ETL Z& DX Jk, ARSE TRIAZ KINA RTL PEAR TTI 
MARS RETTIG ARE. SE Bp Tala, JE D s 20 FF S EE 
EN CHEM H m ARIAETEZJBSRE PRESEN o 


6.2.4 ”小 结 


由 于 不 希望 这 本 书 变 成 一 本 让 人 觉得 头疼 的 数学 教科 书 ， 所 以 这 里 
只 罗列 了 最 常见 的 两 种 非 线性 优化 方案 一 一 高 斯 牛顿 法 和 列 文 伯 格 一 马 
夸 尔 特 方法 。 我 们 避 开 了 许多 数学 性 质 上 的 讨论 。 如 果 读 者 对 优化 感 兴 
累 ， 可 以 进一步 阅读 专门 介绍 数值 优化 的 书籍 (这 是 一 个 很 大 的 课 
题 》， 例 如 [23]。 以 高 斯 牛顿 法 和 列 文 伯 格 一 马 夸 尔 特 方法 为 代表 的 优 
化 方法 ， 在 很 多 开源 的 优化 库 中 都 已 经 实现 并 提供 给 用 户 ， 我 们 会 在 下 
文 进行 实验 。 最 优化 是 处 理 许多 实际 问题 的 基本 数学 工具 ， 不 光 在 视觉 
SLAM 中 起 着 核心 作用 ， 在 类 似 于 深度 学 习 等 其 他 领域 ， 它 也 是 求解 问 
题 的 核心 方法 之 一 。 我 们 希望 读者 能 够 根据 自身 能 力 ， 去 了 解 更 多 的 最 
优化 算法 。 

也 许 你 发 现 了 ， 无 论 是 高 斯 牛顿 法 还 是 列 文 伯 格 一 马 压 尔 特 方法 ， 
在 做 最 优化 计算 时 ， 都 需要 提供 变量 的 初始 值 。 你 也 许 会 问 到 ， 这 个 初 
始 值 能 否 随 意 设置 ? 当然 不 是 。 实 际 上 非 线性 优化 的 所 有 选 代 求解 广 





和 案 ， 痢 需要 用 户 来 提供 一 个 民 好 的 初始 值 。 由 于 目标 函数 太 复 洒 ， 寻 致 
在 求解 衬 间 上 的 变化 难以 琢磨 ， 对 问题 捉 供 不 同 的 初始 值 往往 会 导致 个 
同 的 计算 结果 。 这 种 情况 是 非 线性 优化 的 通病 : 大 多 数 算法 部 容易 陷入 
局 部 极 小 值 。 因 此 ， 无 论 是 哪 类 科学 问题 ， 我 们 提供 初始 值 部 应 该 有 科 
学 依据 ， 例 如 视 兖 SLAM 问 题 中 ， 我 们 会 用 ICP、PnP 之 类 的 算法 提供 优 
化 初始 值 。 总 之 ， 一 个 展 好 的 初始 值 对 最 优化 问题 非常 重要 ! 


也 许 读者 还 会 对 上 面 提 到 的 最 优化 产生 疑问 : 如何 求解 线性 增 量 方 
ZANE? 我 们 只 讲 到 了 增 量 方程 是 一 个 线性 方程 ， 但 是 直接 对 系数 矩阵 
进行 求 揭 总 不 是 要 进行 大 量 的 计算 ? 当然 不 是 。 在 视 党 SLAM 算 法 里 ， 
经 钊 过 到 Ax “的 维度 大 到 好 几 百 或 者 上 和 于， 如 果 你 是 要 做 大 规模 的 视觉 
三 维 重 建 ， 束 会 经 间 发 现 这 个 维 虚 可 以 轻易 达到 几 十 万 甚至 更 高 的 级 
串 。 要 对 那么 大 个 窍 阵 进行 求 逆 是 大 多 数 处 理 需 无 法 负担 的 ， 因 此 存在 
看 许多 针对 线性 方程 组 的 数值 求解 方法 。 在 不 同 的 领域 有 不 同 的 求解 方 
式 ， 但 几乎 没有 一 种 方式 是 直接 求 系数 矩阵 的 逆 ， 我 们 会 及 用 矩阵 分 解 
的 方法 来 解 线 性 方程 ， 例 如 QR、Cholesky 等 分 解 方法 。 这 些 方法 通常 在 
窍 阵 论 守 教科 书 中 可 以 找到 ， 我 们 不 多 加 介绍 。 

对 运 的 是 ， 视 党 SLAM 里 这 个 窍 阵 往往 有 特定 的 稀 焉 形式 ， 这 为 实 
时 求解 优化 问题 提供 了 可 能 性 。 我 们 将 在 第 10 讲 中 详细 介绍 它 的 原理 。 
利用 稀 焉 形式 的 消 元 、 人 分解 ， 了 最 后 再 进行 求解 增 量 ， 会 让 求解 的 效率 大 
大 提高 。 在 很 多 开源 的 优化 库 上 ， 维 度 为 一 万 多 的 变量 在 一 般 的 PC 上 
残 可 以 在 几 秒 甚 至 更 短 的 时 间 内 被 求解 出 来 ， 其 原因 也 是 用 了 更 加 高 级 
的 数学 工具 。 视 党 SLAM 算 法 现在 能 够 实时 地 实现 ， 也 多 亏 了 系数 官 阵 
Fe PEA, BUR AEE ERE, RAI SR DSLAM IE EAN SW 
PEFR) YER AN [ 2425261 。 


6.3 KEk: Ceres 


我 们 前 面 说 了 很 多 理论 ， 现 在 来 实践 一 下 前 面 提 到 的 优化 算法 。 在 
本 讲 的 实践 部 分 中 ， 我 们 主要 同 大 家 介绍 两 个 C++ 的 优化 库 : ORE aK 
的 Ceres 库 ?71 以 及 基于 图 优化 的 g20 库 中 。 由 于 g2o 的 使 用 还 需要 介绍 一 
点 图 优化 的 相关 知识 ， 所 以 我 们 先 来 介绍 Ceres， 然 后 介绍 一 些 图 优化 
理论 ， 最 后 来 讲 g20。 由 于 优化 算法 在 之 后 的 “视觉 里 程 计 ” 和 “后 端 * 中 
都 会 出 现 ， 所 以 请 读者 务必 向 握 优化 算法 的 音义， 理解 程序 的 内 容 。 


6.3.1 ” ”Ceres 简介 


Ceres 库 面 癌 通用 的 最 小 二 乘 问题 的 求解 ， 作 为 用 性， 我 们 需要 做 
的 束 是 定义 优化 问题 ， 然 后 设置 一 些 选项 ， 输 入 进 Ceres 求 解 即 可 。 
Ceres 求 解 的 最 小 二 买 问题 最 一 般 的 形式 如 下 市 边界 的 核 隙 数 最 小 二 
He): 


. 2 
l 


s.t. l; < Lj < Uj. 


可 以 看 到 ， 目 标 函 数 由 许多 平方 项 经 过 一 个 核 函 数 p(: ) 之 后 求 和 组 
成 。 在 最 简单 的 情况 下 ， 取 p 为 恒 等 函 数 ， 则 目标 函数 即 为 许多 项 的 
平方 和 。 在 这 个 问题 中 ， 优 化 变量 为 x yx, ，f 称 为 代价 函数 (Cost 
function) » fESLAM PIP AFR AiR. 1 lu, 为 第 ) 个 优化 变量 的 
上 限 和 下 限 。 在 最 简单 的 情况 下 ， 取 1 =-cou =o (不 限制 优化 变量 的 边 
界 ) ， 并 且 取 p 为 恒 等 函 数 时 ， 就 得 到 了 无 约束 的 最 小 二 乘 问题 ， 和 我 
们 先前 说 的 是 一 致 的 。 

在 Ceres 中 ， 我 们 将 定义 优化 变量 x 和 每 个 代价 函数 [ ， 再 调用 Ceres 
进行 求解 。 我 们 可 以 选择 使 用 高 斯 牛顿 法 或 者 列 文 伯 格 一 马 夸 尔 特 方法 
进行 梯度 下 降 ， 并 设 定 梯度 下 降 的 条 件 ，Ceres 会 在 优化 之 后 将 最 优 估 


(6.27) 


计 值 返回 。 下 和 面 ， 我 们 通过 一 个 曲线 拟 合 的 实验 来 实际 操作 一 下 
Ceres， 理 解 优 化 的 过 程 。 


6.3.2 Zł% Ceres 


73 f H Ceres, Ta 70 mi SET Fae al 建议 去 GitHub 上 下 载 
Ceres: https://github.com/ceres-solver/ceres-solver。 本 书 资 源 的 3rdparty 下 
也 附带 了 Ceres 库 。 

与 之 前 磁 到 的 库 一 样 ，Ceres 是 一 个 cmake 工 程 。 先 来 安 猴 它 的 依赖 
项 ， 在 Ubuntu 中 可 以 用 apt-get 安 荫 ， 主 要 是 谷歌 目 己 使 用 的 一 些 日 志和 
测试 工具 : 






1 | sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags- 
dev libgoogle-glog-dev libgtest-dev 


然后 ， 进 入 Ceres 库 目录 下 ， cmake ji FE Jf ZR 这 个 过 程 我 
CAOL, KAADAA., TI SEAE. 
在 /usr/local/include/ceres 下 找到 Ceres ALF, 3ffE/usr/local/lib/ F Fk 2] 
名 为 libceres.a 的 库 文 件 。 有 了 这 些 文 件 ， 束 可 以 使 用 Ceres 进 行 优 化 计算 
Js 


6.3.3 [EH] Cerest S Hi 2X 


我 们 的 演示 实验 包括 使 用 Ceres 和 接 下 来 的 g20 进 行 曲 线 拟 合 。 假 设 
有 一 条 满足 以 下 方程 的 曲线 : 


y 一 exp(az + br +c) + w, 


其 中 ab,c 为 曲线 的 参数 ，w 为 高 斯 噪声 。 我 们 故意 选择 了 这 样 一 个 
非 线性 模型 ， 以 使 问题 不 至 于 太 简单 。 现 在 ， 假 设 我 们 有 N “个 关于 xy 
的 观测 数据 点 ， 想 根据 这 些 数据 点 求 出 曲线 的 参数 。 那 么 ， 可 以 求解 下 
面 的 最 小 二 乘 问题 以 估计 曲线 参数 ， 


N 
i 2 
piii 5 3 |ui — exp (ax; + bx; + c) | l (6.28) 


请 注意 ， 在 这 个 问题 中 ， 竺 估计 的 变量 是 pc ， 而 不 是 x 。 我 们 写 
一 个 程序 ， 先 根据 模型 生成 x,yy Ae, AJR tE RAE Pas te od A 
RE. 随后， 使 用 Ceres 从 市 噪声 的 数据 拟 合 参 数 模型 。 


四 slambook/ch6/ceres curve fi tting/main.cpp 


1 |#include <iostream> 
2 |#include <opencv2/core/core.hpp> 
3 |#include <ceres/ceres.h> 


4 |#include «chrono» 


6 |using namespace std; 


8 | // 代价 函数 的 计算 模型 

9 | struct CURVE FITTING COST 

i0 |t 

Y CURVE FITTING COST ( double x, double y ) : x (x), y (y) f} 
12 // 残 差 的 计算 


13 template «typename T> 

14 bool operator() ( 

15 const T* const abc, // 模型 参数 ， 有 3H 

16 T* residual ) const // BE 

17 1 

18 // y-exp (az ^2*bz*c) 

19 residual[0] = T ( y ) - ceres::exp ( abclO]*T ( x ) *T ( x ) + abc[1]*T 
( x ) + abc[2] ); 

20 return true; 

21 } 

22 const double x, y; // x,y 数据 


67 


68 


int main ( int argc, char** argv ) 


1 


double a=1.0, b-2.0, c=1.0; // 真实 参数 值 

int N=100; // 数据 点 

double w sigma-1.0; // "kf Sigma 值 
cv::RNG rng; // OpenCV 随机 数 产生 器 
double abc[3] = {0,0,0}; // abc 参数 的 估计 值 
vector<double> x data, y data; // 数据 


cout<<"generating data: "««endl; 


for ( int i20; i<N; i++ ) 


1 
double x = i/100.0; 
x data.push back ( x ); 
y.data.push, back ( 
exp ( a*x*x * b*x * c ) * rng.gaussian ( w sigma ) 
); 
cout««x data[i]««" "««4y data[i]««end1; 
} 


// 构建 最 小 二 乘 问题 
ceres::Problem problem; 


for ( int i20; i<N; i++ ) 


1 
problem.AddResidualBlock ( // 向 问题 中 添加 误差 项 
// 使 用 自动 求 导 ， 模 板 参 数 : 误差 类 型 ， 输 出 维度 ， 输 入 维度 ， 数 值 参 照 前 面 
struct 中 写法 
new ceres::AutoDiffCostFunction«CURVE FITTING COST, 1, 3> (人 
new CURVE FITTING COST ( x data[i], y data[i] ) 
2's 
nullptr, // hk, AERA, AZ 
abc // 待 估 计 参 数 
)5 
} 
// 配置 求解 器 
ceres::Solver::Üptions options; // 这 里 有 很 多 配置 项 可 以 填 
options.linear solver type = ceres::DENSE QR; // 增 量 方程 如 何 求解 
options.minimizer_progress_to_stdout = true; // ii dj cout 
ceres::Solver::Summary summary; // 优化 信息 


chrono::steady clock::time point tí = chrono::steady_clock: :now() ; 
ceres::Solve ( options, &problem, &summary );  // 开始 优化 


chrono::steady clock::time point t2 - chrono::steady clock::now(); 


69 chrono::duration<double> time used = chrono::duration_cast<chrono: :duration< 
double>>( t2-t1 ); 


70 cout<<"solve time cost = "««time used.count()««" seconds. "««endl; 


72 // 输出 结果 


73 cout<<summary.BriefReport() ««endl; 
74 cout<<"estimated a,b,c = "; 

75 for ( auto a:abc ) cout<<a<<" "; 

76 cout««endl; 

77 

78 return 0; 

7 |} 





程序 中 需要 说 明 的 地 方 均 已 加 注释 。 可 以 看 到 ， 我 们 利用 OpenCV 
的 噪声 生成 费 生成 了 100 个 之 局 斯 噪声 的 数据 ， 随 后 利用 Ceres 进 行 拟 
合 。Ceres 的 用 法 如 下 : 

1. 定 义 Cost ”Function 模 型 。 方 法 是 书写 一 个 类 ， 并 在 类 中 定义 禹 模 
TU EX moi. KBAR RA I — Pea (Functor, CN 
BO 。 这 种 定义 方式 使 得 Ceres 可 以 像 调 用 函数 一 样 ， 对 广 关 的 采 个 对 
象 〈 比 如 a) 调用 a<double>0) 方 法 一 一 这 使 对 象 具有 像 函 数 那 样 的 行 

2. 调 用 AddResidualBlock 将 误 盈 项 添加 到 目标 函数 中 。 由 于 优化 需 
要 梯度 ， 我 们 有 知 王 种 选择 : COD 使 用 Ceres 的 自动 求 导 CAuto Di 
ff) ; (2) 使 用 数值 求 导 (Numeric Di ff): G) 目 行 推导 解析 的 导 
数 形式 ， 提 供给 Ceres。 其 中 目 动 求 导 在 编 色 上 是 最 方便 上 时， 于 是 我 们 
使 用 自动 求 导 。 

3. 目 动 求 导 需要 指定 误差 项 和 优化 变量 的 维度 。 这 里 的 误 和 是 标 
m=, RNL: 优化 的 是 ap,c 三 个 量 ， 维 度 为 3。 于 是 ， 在 目 动 求 导 类 的 
模板 参数 中 设 定 变量 维度 为 1、3。 

4. 设 定好 问题 后 ， 调 用 solve 国 数 进 行 求解 。 你 可 以 在 options 里 配置 
(非常 详细 的 ) 优化 选项 。 例 如 ， 可 以 选择 使 用 Line Search Æ Trust 
Region, IARR Kk, Se. AUAA Optioosh EX, BAA 
哪些 优化 方法 可 选 ， 当 然 默认 的 配置 已 经 可 用 于 很 广泛 的 问题 了 。 


最 后 ， 我 们 来 看 看 实验 结果 。 调 用 build/curve fi tting 查 看 优化 结 





1 |% build/curve fitting 
2 |generating data: 

3 |0 2.71828 

4 |0.01 2.93161 

5 |0.02 2.12942 

6 |0.03 2.46037 


s | iter cost cost_change |gradient | |step| tr_ratio tr_radius ls_iter 


iter_time total_time 


9 |O 1.824887e+04 0.00e*00 1.38e+03 0.00e+00  0.00e*00 1.00e+04 0 
4.09e-05 1.48e-04 

10 |1 2.748700e+39 . -2.75e*39 0.00e+00 7.67e*01 -1.52e+35 5.00e+03 1 
1.09e-04 3.13e-04 

n |2 2.429783e+39 . -2.43e*39 0.00e+00 7.62e*01 -1.35e*35 1.25e+03 1 


3.57e-05 | 3.75e-04 


i3 |18 65.310764e+01 3.42e*00 8.50e+00 2.81e-01 9.89e-01 2.53e+03 1 
3.09e-05 1.15e-03 

4 |19 5.125939e+01 1.85e+00 2.84e+00 2.98e-01 9.90e-01 7.60e+03 1 
2.85e-05  1.19e-03 

15 |20 5.097693e+01 2.82e-01 4.34e-01 1.48e-01 9.95e-01 2.28e+04 1 
2.82e-05 1.23e-03 

16 |21 5.096854e+01 8.39e-03 3.24e-02 2.87e-02 9.96e-01 6.84e+04 1 


3.04e-05 1.27e-03 

17 | solve time cost = 0.00133349 seconds. 

18 | Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e+04, Final cost: 
5.096854e+01, Termination: CONVERGENCE 

19 |estimated a,b,c = 0.891943 2.17039 0.944142 


MA Ceres tH BS Do Hai ULES, EPRSVARZSXZ39418248 FS] T 
50.9, FHE PERKER. TEXETR22U RAM, rmn gui t 
7j 


a = 0.891943, 5 — 2.17039, e= 0.944142. 


而 我 们 设 定 的 真 值 为 


它们 相 甜 不 多 。 


为 了 更 直观 地 显示 数据 ， 可 以 把 它 画 出 来 ， 如 图 6-1 所 示 。 其 中 显 
示 了 市 噪声 的 数据 、 真 实 模 型 和 估计 模型 ， 可 以 看 到 估计 模型 和 真实 模 
型 非常 接近 ， 几 乎 重合 。 我 们 同时 记录 了 Ceres 的 运行 时 间 ， 对 这 样 一 
个 100 个 点 的 优化 问题 ， 计 算 时 间 约 1.3ms (虚拟 机 上 )。 


硕 望 谈 者 通过 这 个 人 简单 的 例子 对 Ceres 的 使 用 方法 有 一 个 大 致 了 
解 。 它 的 优点 是 提供 了 目 动 求 导 工 具 ， 使 得 不 必 去 计算 很 矿 烦 的 雅 可 比 
FEM. Ceres) Baye SERA OSLO, «FES PEAY HA Ay DA SE pk 
目 动 求 导 工作 ， 不 过 仍然 是 数值 导数 。 本 书 大 部 分 时 候 仍 然 会 介绍 雅 可 
比 窍 阵 的 计算 ， 因 为 那样 对 理解 问题 更 有 帮助 ， 而 且 在 优化 中 更 少 出 现 
问题 。 此 外 ，Ceres 的 优化 过 程 配 置 也 很 丰 军 ， 使 其 适合 很 广泛 的 最 小 
二 乘 优化 问题 ， 包 括 SLAM 中 的 各 种 问题 。 





一 0 
一 0.2 0.0 0.2 0.4 0.6 0.8 1.0 1.2 


图 6-1 ”使 用 Ceres 进 行 曲线 拟 合 。 真 实 模型 和 估计 模型 非常 接近 。 


6.4 实践: g2o 


本 讲 的 第 2 个 实践 部 分 将 介绍 另 一 个 “主要 在 SLAM 领 域 ) 广 为 使 
用 的 优化 库 : g20 (General Graphic Optimization, G? O) 。 它 是 一 个 基 
于 图 优化 ”的 库 。 图 优化 是 一 种 将 非 线 性 优化 与 图 论 结合 起 来 的 理论 ， 
因此 在 使 用 它 之 前 ， 我 们 花 一 点 篇 幅 介 绍 一 下 图 优化 理论 。 


6.4. 图 优化 理论 简介 


我 们 已 经 介绍 了 非 线性 最 小 二 乘 的 求解 方式 。 它 们 是 由 很 多 个 误差 
项 之 和 组 成 的 。 然 而 ， 仅 有 一 组 优化 变量 和 许多 个 误差 项 ， 我 们 并 不 清 
楚 它们 之 间 的 关联 。 比 如 ， 某 个 优化 变量 x 存在 于 多 少 个 误差 项 中 呢 ? 
我 们 能 保证 对 它 的 优化 是 有 意义 的 吗 ? 进一步 ， 我 们 希望 能 够 直观 地 看 
到 该 优化 问题 长 什么 样 。 于 是 ， 就 牵涉 到 了 图 优化 。 


图 优化 ， 是 把 优化 问题 表现 成 图 ( Graph) 的 一 种 方式 。 这 里 的 图 
是 图 论 意 义 上 的 图 。 一 个 图 由 在 干 个 项 点 〈 Vertex) ” ， 以 及 连接 看 这 
些 顶 点 的 边 〈 Edge) 组 成 。 进 而 ， 用 顶点 表示 优化 变量 ， 用 边 表示 
RAM ”。 于 是 ， 对 任意 一 个 上 述 形 式 的 非 线 性 了 好 小 二 乘 问题 ， 我 们 可 
以 构建 与 之 对 应 的 一 个 图 。 


图 6-2 下 一 个 徐 单 的 图 优化 例子 。 我 们 用 三 角形 表示 相机 位 姿 节 

扩 ， 用 圆 形 表示 路 标点 ， 它 们 构成 了 图 优化 的 项 上 号 ， 同 时 ， 实 线 表 示 相 
机 的 运动 模型 ， 虚 线 表 示 观 测 模 型 ， 它 们 构成 了 图 优化 的 边 。 此 时 ， 虽 
然 整个 问题 的 数 竺 形式 仍 是 式 (6.12) 那 样 ， 但 现在 我 们 可 以 丰 观 地 看 到 

问题 的 结构 o WREE, Wa A EIOM 或 优先 优化 边 数 较 
多 《或 按 图 论 的 术语 ， 度 数 较 大 ) 的 顶 上 ”这样 的 改进 。 但 是 最 其 本 的 
图 优化 是 用 图 模型 来 表达 一 个 非 线 性 最 小 二 乘 的 优化 问题 。 而 我 们 可 以 
利用 图 模型 的 茶 些 性 质 做 更 好 的 优化 。 
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图 6-2 ”图 优化 的 例子 。 


g20 为 SLAM 提 供 了 图 优化 所 需 的 内 容 。 下 面 浇 示 一 下 g20 的 使 用 方 
s 


6.4.2 ”g2o 的 编译 与 安装 


在 使 用 一 个 库 之 前 ， 我 们 需要 对 它 进 行 编 译 和 安装 。 旋 者 应 该 已 经 
体验 过 很 多 次 这 种 过 程 了 ， 它 们 基本 大 同 小 异 。 关 于 g20， 谈 者 可 以 从 
GitHub 下 载 它 : https://github.com/RainerKuemmerle/g20， 或 从 本 书 提供 
HJ 95 — 73 fX R8 Eg HARE o 

解压 代 人 码 包 后 ， 你 会 看 到 g20o0 库 的 所 有 源码， 它 也 是 一 个 cmake 工 
程 。 我 们 先 来 安装 它 的 依赖 项 (部 分 依赖 项 与 Ceres 章 合 ) : 


sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev 


libcxsparse3.1.2 libcholmod-dev 
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程 的 说 明 。 安 装 完 成 后 ，g20 的 涉 文 件 将 位 于 /usr/local/g20 下 ， 库 文件 位 
ee ly 现在 ， 我 们 重新 考虑 Ceres 例 程 中 的 曲线 拟 合 实验 ， 
在 g20 中 实验 一 


6.4.3 ”使 用 g2o 拟 合 曲线 


为 了 使 用 g2o， 首 先 要 将 曲线 拟 合 问 题 抽象 成 图 优化 。 这 个 过 程 
H, ABET RATA, WARM 即 可 。 曲 线 拟 合 的 图 优化 
问题 可 以 画 成 图 6-3 的 形式 。 


C 待 估计 的 参数 构成 了 节点 
见 测 数 据 构成 了 边 


图 6-3 ”曲线 拟 合 对 应 的 图 优化 模型 。 (英明 其 妙 地 有 些 像 华为 的 标志 ) 


在 曲线 拟 合 问题 中 ， 你 个 问题 只 有 一 个 项 点 : 曲线 模型 的 参数 q,b,c 
; 而 各 个 帘 品 声 的 数据 点 ， 构 成 了 一 个 个 误 兰 项 ， 也 了 吏 是 图 优化 的 辽 。 
但 这 里 的 边 与 我 们 平时 想 的 边 不 太一 样 ， 它 们 是 一 元 边 (Unary 
Edge) ， 即 只 连接 一 个 顶点 ”一 一 因为 整个 图 只 有 一 个 项 点。 所 以 在 图 
6-3 中 ， 我 们 只 能 把 它 画 成 目 己 连 到 目 己 的 样子 。 事 实 上 ， 图 优化 中 一 
条 边 可 以 连接 一 个 、 两 个 或 多 个 项 点， 这 主要 反映 每 个 误 天 与 多 少 个 优 
化 变量 有 天 。 在 稍 有 些 玄妙 的 说 法 中 我 们 把 它 叫 作 超 边 (Hyper 
Edge) ， 整 个 图 叫 作 超 图 (Hyper Graph) ?! . 


弄 消 了 这 个 图 模型 之 后 ， 接 下 来 就 是 在 g20 中 建立 该 模型 进行 优化 


了 。 作 为 g2o 的 用 户 ， 我 们 要 做 的 事主 要 包含 以 下 步 又 : 
1. 定 义 顶 点 和 这 的 类 型 。 
2. 构 建 图 。 
3. 选 择优 化 算法 。 
4. 调 用 g2o 进 行 优 化 ， 返 回 结束 。 
PB BYR AN BREF o 
四 slambook/ch6/ g20 curve fi tting/main.cpp 


#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


<iostream> 

<g20/core/base_vertex.h> 
<g20/core/base_unary_edge.h> 
<g20/core/block_solver.h> 
<g20/core/optimization_algorithm_levenberg.h> 
<g20/core/optimization_algorithm_gauss_newton.h> 
<g20/core/optimization_algorithm_dogleg.h> 
<g20/solvers/dense/linear_solver_dense.h> 
<Eigen/Core> 

<opencv2/core/core.hpp> 

<cmath> 


<chrono> 


using namespace std; 


// 曲线 模型 的 顶点 ， 模 板 参 数 : 优化 变量 维度 和 数据 类 型 


class CurveFittingVertex: public g20o::BaseVertex«3, Eigen::Vector3d» 


EIGEN, MAKE, ALIGNED. OPERATOR, NEW 
virtual void setToOriginImpl() // £ Ë 


estimate «« 0,0,0; 


virtual void oplusImpl( const double* update ) // 更 新 


estimate *- Eigen::Vector3d(update); 


// FRA: OW 


virtual bool read( istream& in ) {} 


virtual bool write( ostream& out ) const {} 


1 
public: 
1 
} 
{ 
} 
}; 


// 误差 模型 模板 参数 : 
class CurveFittingEdge: public g20o::BaseUnaryEdge«1,double,CurveFittingVertex»? 


1 
public: 


观测 值 维度 ， 类 型 ， 连 接 顶 点 类 型 


EIGEN MAKE ALIGNED OPERATOR NEW 


CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {} 


// 计算 曲线 模型 误差 


void computeError() 


{ 


const CurveFittingVertex* v = 


Vertices [0] ) ; 


static cast«const CurveFittingVertex*> ( 
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const Eigen::Vector3d abc = v-»estimate(); 
.error(0,0) = measurement - std::exp( abc(0,0)* x* x + abc(1,0)* x + abc 
(2,0) ) ; 

) 

virtual bool read( istream& in ) {} 

virtual bool write( ostream& out ) const {} 

public: 
double x; // x fil, y 值 为 _measurement 


$s 


int main( int argc, char** argv ) 


{ 
double a=1.0, b=2.0, c=1.0; // 真实 参数 值 
int N-100; // 数据 点 
double w sigma-1.0; // *k P Sigmalli 
cv: :RNG rng; // 0penCV 随 机 数 产 生 器 
double abc[3] = {0,0,0}; // abc 参 数 的 估计 值 
vector<double> x_data, y_data; // 数据 


cout<<"generating data: "««endl; 


for ( int i20; i<N; i++ ) 


;! 
double x = i/100.0; 
x data.push back ( x ); 
y.data.push back ( 
exp ( a*x*x * b*x * c ) * rng.gaussian ( w sigma ) 
); 
cout««x data[i]««" "<<y_data[i]<<end1; 
} 


// 构建 图 优化 ， 先 设 定 g2o 

// EEk: 每 个 误差 项 优化 变量 维度 为 3 ， 误 差 值 维度 为 1 

typedef g20::BlockSolver< g20o::BlockSolverTraits«3,1» > Block; 

// 线性 方程 求解 器 : 稠密 的 增 量 方程 

Block: :LinearSolverType* linearSolver = new g20o::LinearSolverDense«Block:: 
PoseMatrixType>() ; 

Block* solver_ptr = new Block( linearSolver ); // 和 矩阵 块 求 解 器 
// 梯度 下 降 方法 ， 从 GNW，LNH，DogLeg 中 选 
g20::OptimizationAlgorithmLevenberg* solver = new g2o:: 
OptimizationAlgorithmLevenberg( solver_ptr ); 

// 取消 下 面 的 注释 以 使 用 GN 或 DogLeg 

// g20o::ÜptimizationAlgorithmGaussNewton* solver = new g20:: 
ÜptimizationAlgorithmGaussNeuton( solver_ptr ); 


// g20::OptimizationAlgorithmDogleg* solver = new g2o:: 


OptimizationAlgorithmDogleg( solver_ptr ); 


g20::SparseOptimizer optimizer; // 图 模型 
optimizer.setAlgorithm( solver ); // 设置 求解 器 
optimizer.setVerbose( true ); // 打开 调试 输出 


// 向 图 中 增加 顶点 

CurveFittingVertex* v = new CurveFittingVertex(); 
v->setEstimate( Eigen::Vector3d(0,0,0) ); 
v-»setId(0); 


optimizer.addVertex( v ); 


// 向 图 中 增加 边 


for ( int i=0; i<N; i++ ) 


{ 
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] ); 
edge->setId(i) ; 
edge->setVertex( 0, v ); // 设置 连接 的 顶点 
edge->setMeasurement( y data[i] ) ; // 观测 数值 
// 信息 和 矩阵: 协 方差 矩阵 之 道 
edge->setInformation( Eigen: :Matrix<double,1,1>::Identity() *1/(w_sigma* 
w_sigma) ); 
optimizer.addEdge( edge ); 

} 


// 执行 优化 

cout<<"start optimization"««endl; 

chrono::steady clock::time point tí = chrono::steady clock::now(); 
optimizer.initializeOptimization() ; 

optimizer. optimize (100) ; 

chrono::steady clock::time point t2 = chrono::steady clock::now(); 
chrono::duration«double^» time used = chrono::duration_cast<chrono: :duration< 
double>>( t2-t1 ); 


cout<<"solve time cost = "<<time_used.count()<<" seconds. "««endl; 


// 输出 优化 值 
Eigen::Vector3d abc estimate = v->estimate() ; 


cout<<"estimated model: "««abc estimate.transpose()««endl; 


return 0; 


在 这 个 程序 中 ， 我 们 从 g20o 派 生出 了 用 于 曲线 拟 合 的 图 优化 顶点 和 
边 : CurveFittingVertex 和 CurveFittingEdge， 这 实质 上 扩展 了 g2o 的 使 用 
方式 。 在 这 两 个 派生 关中， 我 们 重 写 了 重要 的 虚 函 数 : 

1. 顶 点 的 更 新 图 数 : oplusImpl。 我 们 知道 优化 过 程 最 重要 的 是 增 量 
Ax 的 计算 ， 而 该 图 数 处 理 的 是 x ., =x, +Ax 的 过 程 。 读 者 也 许 觉得 这 并 
不 是 什么 值得 一 提 的 事情 ， 因 为 仅仅 是 个 徐 单 的 加 法 而 已 ， 为 什么 g20 
不 帮 我 们 完成 呢 ? 在 曲线 拟 合 过 程 中 ， 由 于 优化 变量 〈 曲 线 参数 ) A 
位 于 回 量 空间 ” 中， 这 个 更 新 计算 确实 惑 是 简单 的 加 法 。 但 是 ， 当 优化 
赤 量 不 在 回 量 空间 中 时 ， 比 如 说 x 是 相机 位 姿 ， 它 本 和 续 不 一 定 有 加 法 运 
算 。 这 时 ， 束 需要 重 独 定义 增 量 如 何 加 到 现 有 的 估计 上 BJTZJI. t 
照 第 4 讲 的 解释 ， 我 们 可 能 使 用 左 乘 更 新 或 右 滋 更 新 ， 而 不 是 直接 的 加 
法 。 

2. 顶 点 的 重 置 函数 : setToOriginImpl。 这 是 平凡 的 ， 我 们 把 估计 值 
置 零 即 可 。 

3. 边 的 误差 计算 函数 : computeError。 该 函数 需要 取出 边 所 连接 的 
顶点 的 当前 估计 值 ， 根 据 曲 线 模型 ， 与 它 的 观测 值 进行 比较 。 这 和 最 小 
二 乘 问 题 中 的 误差 模 型 是 一 致 的 。 

4. 存 机 和 读 租 函数 : read、write。 由 于 我 们 并 不 想 进 行 读 / 写 操 作 ， 
所 以 留 空 。 

定义 了 顶点 和 边 之 后 ， 我 们 在 main 函 数 里 声明 了 一 个 图 模型 ， 然 后 
按照 生成 的 噪声 数据 ， 往 图 模型 中 添加 顶点 和 边 ， 最 后 调用 优化 函数 进 
行 优化 。g20 会 给 出 优化 的 结 


1 | 4 build/curve fitting 
2 |generating data: 

3 |O 2.71828 

4 [0.01 2.93161 

5 |0.02 2.12942 


7 | iteration- 13 chi2- 101.937020 time- 4.06e-05 cumTime- 0.00048135  edges- 100 
schur= 0 lambda= 3678.088107 levenbergIter= 6 


8 | iteration- 14 chi2= 101.937020 time- 3.2215e-05 cumTime= 0.000513565 edges- 100 
schur- 0 lambda- 19616.469906 levenbergIter- 3 

9 |iteration- 15 chi2- 101.937020 time- 0.000108524 cumTime- 0.000622089 edges- 100 
schur= 0 lambda- 836969.382664 levenberglter= 4 

i0 | iteration- 16 chi2- 101.937020 time- 0.000159817 cumTime- 0.000781906 edges- 100 
schur= 0 lambda- 224672257893341.656250 levenbergIter- 7 

1 | solve time cost = 0.00173976 seconds. 

12 | estimated model: 0.890911 2.1719 0.943629 
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目 行 对 比 各 种 柳 度 下 降 方 法 的 差异 。 


65 ”小 结 


本 节 介 绍 了 SLAM 中 经 第 储 到 的 一 种 非 线性 优化 问题 : 由 许多 个 误 
甜 项 平方 和 组 成 的 最 小 二 乘 间 题 。 我 们 介绍 了 它 的 定义 和 求解 ， 并 且 讨 
论 了 两 种 主要 的 梯度 下 降 方 式 : 高 斯 牛 贺 法 和 列 文 介 格 一 马 伪 尔 特 方 
法 。 在 实践 部 分 中 ， 分 别 使 用 了 Ceres 和 g2o 两 种 优化 库 求解 同一 个 曲线 
拟 合 问题 ， 发 现 它们 给 出 了 相似 的 结 


由 于 还 没有 详细 谈 Bundle Adjustment， 所 以 实践 部 分 选择 了 曲线 拟 
合 这 样 一 个 简单 但 有 代表 性 的 例子 ， 以 污 示 一 般 的 非 线 性 最 小 二 乘 求 解 
方式 。 特 别 地 ， 如 果 用 g2o 来 拟 合 曲线 ， 必 须 先 把 问题 转换 为 图 优化 ， 
定义 新 的 项 点 和 边 ， 这 种 做 法 是 有 一 些 迁 回 的 g20 的 主要 目的 并 不 
在 此 。 相 比 之 下 ，Ceres 定 义 误 午 项 求 曲 线 拟 合 问题 则 目 然 了 很 多 ， 
为 它 本 刁 即 是 一 个 优化 库 。 然 和 而， 在 SLAM 中 更 多 的 问题 是 ， 一 个 市 有 
许多 个 相机 位 姿 和 许多 个 空间 点 的 优化 问题 如 何 求 解 。 特 别 地 ， 当 相机 
位 姿 以 李 代 数 表示 时 ， 误 委 项 关于 相机 位 姿 的 导数 如 何 计算 ， 将 是 一 件 
值得 详细 讨论 的 事 。 我 们 将 在 后 续 内 容 友 现 ，g2o 提 供 了 大 量 的 项 点 和 
边 的 类 型 ， 非 党 便于 相机 位 姿 佑 计 问 题 。 而 在 Ceres 中 ， 我 们 不 得 不 目 
己 实 现 每 一 个 Cost Function， 有 一 些 不 便 。 

在 实践 部 分 的 两 个 程序 中 ， 我 们 没有 去 计算 曲线 模型 天 于 三 个 参数 
的 导数 ， 而 是 利用 了 优化 库 的 数值 求 导 ， 这 使 得 理论 和 代码 都 会 简洁 一 
些 。Ceres 库 提供 了 基于 模板 元 的 目 动 求 导 和 运行 时 的 数值 求 寻 ， 而 g20 
只 提供 了 运行 时 数值 求 寻 这 一 种 方式 。 但 是 ， 对 于 大 多 数 问题 ， 如 条 能 
够 推导 出 雅 可 比 矩 阵 的 解析 形式 并 告诉 优化 库 ， 就 可 以 避免 数值 求 导 中 
的 诸多 问题 。 

最 后 ， 和 希望 读者 能 够 适应 Ceres 和 g2o 这 些 大 量 使 用 模板 编程 的 方 
式 。 也 许 一 开始 会 看 上 去 比较 吓人 “特别 是 Ceres 设 置 Problem 和 g2o 初 始 
化 部 分 的 代码 ) ， 但 是 鸭 芒 之 后 ， 职 会 觉得 这 样 的 方式 是 目 然 的 ， 而 且 
容易 扩展 。 我 们 将 在 SLAM 后 新 一 讲 中 继续 讨论 稀 艳 性 、 核 函数 、 位 姿 
| (Pose Graph) 等 问题 。 


7] ie 





1. 证 明 线 性 方程 Ax =b 当 系 数 和 矩阵 A 超 定 时 ， 最 小 二 乘 解 为 x =(ATA 
AD。 

2. id Wt iB BEY ^O. rere uu yx fHtis-— ARE 
法 各 有 什么 优 缺 点 。 除 了 我 们 举 的 Ceres 库 和 g2o 库 ， 还 有 哪些 篆 用 的 优 
WE? “你 可 能 会 找到 一 些 MATLAB 上 的 库 。) 

3. 为 什么 高 斯 牛顿 法 的 增 量 方程 系数 矩阵 可 能 不 正定 ? 不 正定 有 什 
么 几何 合 义 ?为 什么 在 这 种 情况 下 解 束 不 稳定 了 ? 

4.DogLeg 是 什么 ? 它 与 高 斯 牛顿 法 和 列 文 介 格 一 马 僵 尔 特 方法 有 何 
异同 ”请 搜索 相关 的 材料 中 。 

5. 阐 谈 Ceres 的 教学 材料 Chttp://ceres-solver.org/tutorial.html) 以 更 好 
地 掌握 其 用 法 。 

6. 阅 读 g20 目 市 的 文档 ， 你 能 看 异 它 吗 ? 如 果 还 不 能 完全 看 虱 ， 请 
在 第 10 讲 和 第 11 讲 之 后 回来 再 看 。 

7* 请 更 改 曲 线 拟 合 实验 中 的 曲线 模型 ， 并 用 Ceres 和 g20 进 行 优化 实 
验 。 例 如 ， 可 以 使 用 更 多 的 参数 和 更 复杂 的 模型 。 





[1] 核 函数 的 详细 讨论 见 第 10 讲 。 
[2] 虽然 笔者 个 人 并 不 太 豆 欢 有 些 故 和 弄 玄 虚 的 说 法 。 





[3] #40, http://www.numerical.rl.ac.uk/people/nimg/course/lectures/raphael/lectures/lec7slides.pdf . 
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主要 目标 
1. 理 解 图 像 符 征 点 的 意义 ,并 笛 握 在 单 幅 图 像 中 提取 出 特征 点 及 多 幅 
图 像 中 匹配 特征 后 的 方法 。 


2. 理 解 对 极 几 何 的 原理 ， 利 用 对 极 几 何 的 约束 ， 恢 复出 图 像 之 间 的 
摄像 机 的 三 维 运动 。 

3. 理 解 PNP 问 题 ， 以 及 利用 已 知 三 维 结构 与 图 像 的 对 应 关系 求解 摄 
像 机 的 三 维 运动 。 

4. 理 解 ICP 问 题 ， 以 及 利用 点 云 的 匹配 关系 求解 摄像 机 的 三 维 运 
动 。 

5. 理 解 如 何 通 过 三 角 化 获得 二 维 图 像 上 对 应 点 的 三 维 结构 。 

本 书 前 面 介 绍 了 运动 方程 和 观测 方程 的 具体 形式 ， 并 讲解 了 以 非 线 
性 优化 为 主 的 求解 方法 。 从 本 讲 开始 ， 我 们 结束 基础 知识 的 铺垫 而 步 入 
正题 : 按照 第 2 讲 的 内 容 ， 分 别 介 绍 视 觉 里 程 计 、 优 化 后 端 、 回 环 检测 
和 地 图 构 建 4 个 模块 。 本 讲 和 下 一 讲 主要 介绍 作为 视 党 里 程 计 的 主要 理 
论 ， 然 后 在 第 9 讲 中 进行 一 次 实践 。 本 讲 关 注 基 于 特征 点 方式 的 视觉 里 
程 计 算法 。 我 们 将 介绍 什么 是 特征 点 、 如 何 提取 和 [匹配 特征 点 ， 以 及 如 
何 根 据 配 对 的 特征 点 估计 相机 运动 。 





7.1 特征 点 法 


回顾 第 2 讲 的 内 容 ， 我 们 说 过 视觉 SLAM 主 要 分 为 视觉 前 靖 和 优化 

后 端 。 前 端 也 称 为 视觉 里 程 计 CVO) 。 它 根据 相 邻 图 像 的 信息 估计 出 
粗略 的 相机 运动 ， 给 后 闯 捉 供 较 好 的 初始 值 。VO 的 实现 方法 ， 鬼 坪 合 
再 要 提 取 特征 ， 分 为 特征 点 法 的 前 闯 及 不 提 特 征 的 直接 法 前 站。 基于 特 
IERARH m KAAR EIME) BU Are Ait BRETT AYE iT 

法 。 它 运行 稳定 ， 对 光照 、 动 态 物 体 不 敏感 ， 是 目前 比较 成 熟 的 解决 方 
条 。 在 本 讲 中 ， 我 们 将 从 特征 点 法 入 手 ， 学 习 如 何 皖 取 、 匹 配 图 像 特 征 
所， 然后 估计 两 帧 之 则 的 相机 运动 和 场景 结构 ， 从 而 实现 一 个 基本 的 两 
li FH] JUS E FETE o 


7.1.1 特征 点 


VO 的 主要 问题 是 如 何 根据 图 像 来 估计 相 机 运动 。 然 而 ， 图 像 本 时 
是 一 个 由 有 党 度 和 色彩 组 成 的 窍 了 省， 如果 生 接 从 窍 阵 层面 考虑 运动 信 计 ， 
将 会 非 党 困难。 所 以 ， 我 们 习惯 于 采用 这 样 一 种 做 法 : Hoc. MEME 
选取 比较 有 代表 性 的 点 。 这 些 点 在 相机 视角 友 生 少量 变化 后 会 你 持 不 
变 ， 所 以 我 们 会 在 各 个 图 像 中 找到 相同 的 后。 然后 ， 在 这 些 操 的 基础 
上 ， 讨 论 相机 位 姿 信 计 问 题 ， 以 及 这 些 操 的 定位 问题 。 在 经 典 SLAM 桩 
型 中 ， 把 它们 称 为 路 标 。 而 在 视 涡 SLAM 中 ， 路 标 则 走 指 图 像 特 征 


(Feature) 。 


根据 维基 百科 有 的 定义 ， 图 像 特征 是 一 组 与 计算 任务 相关 的 信息 ， 计 
算 任务 取决 于 具体 的 应 用 嘲 ”。 们 而 言 之 ， 特 征 古 图 像 信息 的 为 一 种 数 
字 表 达 形 式 ”。 一 组 好 的 特征 对 于 在 指定 任务 上 的 最 终 表 现 全 天 重要 ， 
所 以 多 年 来 研究 者 们 伦 费 了 大 量 的 精力 对 特征 进行 研究 。 数 子 图 像 在 计 
算 机 中 以 灰 度 值 窍 阵 的 方式 存储， 所 以 最 简单 的 ， 单 个 图 像 像 系 也 古 一 
种 “特征 >。 但 是 ， 在 视 党 里 程 计 中 ， 我 们 希望 特征 点 在 相机 运动 之 后 伍 
持 稳定 ”， 而 灰 度 值 受 光照 、 形 变 、 物 体 材 质 的 影响 严 乍 ， 在 不 同 图 像 
间 变 化 非常 大 ， 不 够 稳定 。 理 想 的 情况 是 ， 当 场景 和 相机 视角 及 生 少 量 
改变 时 ， 还 能 从 图 像 中 判断 哪些 地 方 是 同一 个 点 ， 因 此 仅 攒 灰 度 值 是 不 


够 的 ， 我 们 需要 对 图 像 手 取 特 征 点 。 


特征 点 是 图 像 里 一 些 特别 的 地 方 。 以 图 7-1 为 例 。 我 们 可 以 把 图 你 
中 的 角 点 、 边 缘 和 区 块 都 当成 图 像 中 有 代表 性 的 地 方 。 不 过 ， 我 们 更 容 
易 精确 地 指出 ， 某 两 幅 图 像 中 出 现 了 同一 个 角 点 ， 同 一 个 边缘 则 稍微 困 
难 一 些 ， 因 为 沿 着 该 边缘 前 进 ， 图 像 局 部 是 相似 的 ， 同 一 个 区 块 则 是 最 
困难 的 。 我 人 发现， 图像 中 的 角 点 、 边 缘 相 比 于 像素 区 块 而 言 更 加 * 特 
别 "， 在 不 同 图 像 之 间 的 辨识 度 更 强 。 所 以 ， 一 种 直观 的 提取 特征 的 方 
式 就 是 在 不 同 图 像 间 辨认 角 点 ， 确 定 它们 的 对 应 关系 。 在 这 种 做 法 中 ， 
角 点 就 是 所 谓 的 特征 。 

然而 ， 在 大 多 数 应 用 中 ， 单 纯 的 角 点 依然 不 能 满足 我 们 的 很 多 需 
求 。 例 如 ， 从 远 处 看 上 去 是 角 点 的 地 方 ， 当 相机 走 近 之 后 ， 可 能 就 不 显 
示 为 角 点 了 。 或 者 ， 当 旋转 相机 时 ， 角 点 的 外 观 会 发 生变 化 ， 我 们 也 就 
不 容易 辨认 出 那 是 同一 个 角 点 。 为 此 ， 计 算 机 视觉 领域 的 研究 者 们 在 长 
年 的 研究 中 设计 了 许多 更 加 稳定 的 局 部 图 像 特 征 ， 如 著名 的 SIFTB 
SURFE! 、ORBB2 ， 等 等 。 相 比 于 朴素 的 角 点 ， 这 些 人 工 设计 的 特征 点 
能 够 拥有 如 下 的 性 质 ， 


1. 可 重复 性 (Repeatability) : 相同 的 “区 域 ” 可 以 在 不 同 的 图 像 中 找 
到 |。 


2. 可 区 别 性 〈Distinctiveness) : 不 同 的 “区 域 * 有 不 同 的 表达 。 


3. AU CE ffi ciency) : 同一 图 像 中 ， 特 征 点 的 数量 应 远 小 于 保 
系 的 数量 。 
4. 本 地 性 (Locality〉: 特征 仪 与 一 小 厂 图 像 区 域 相关 。 





图 7-1 可 以 作为 图 像 特征 的 部 分 : 角 点 、 边 绿 、 区 块 


特征 点 由 关键 点 — CKey-poinO 和 描述 子 (Descriptor) 两 部 分 组 
成 。 比 如 ， 妆 谈论 SIFT 特 征 时 ， 是 指 “ 提 取 SIFT 关 键 点 ， 并 计算 SIFT 摘 
述 子 ”两 件 事情 。 关 键 点 古 指 该 特征 点 在 图 像 里 的 位 置 ， 有 些 特征 点 还 
具有 随同 、 大 小 等 信息 。 拉 述 子 通 和 常 是 一 个 同 量 ， 按 照 菜 种 人 为 设计 的 
方式 ， 接 述 了 该 关键 点 周围 像 系 的 信息 。 摘 述 子 是 按照 “外 观 相 似 的 特 
征 应 该 有 相似 的 描述 子 ”的 原则 设计 上 的。 因此， 只 要 两 个 特征 点 的 摘 述 
子 在 回 量 空间 上 的 距离 相近 ， 允 可 以 认为 它们 是 同样 的 特征 点 。 

历史 上 ， 研 究 者 们 提出 过 许多 图 像 特征 。 它 们 有 些 很 精确 ， 在 相机 
的 运动 和 光照 变化 下 仍 具 有 相似 的 表达 ， 但 相应 地 需要 较 大 的 计算 量 。 
其 中 ，SIFT (REDERIER, Scale-Invariant Feature Transform) 当 
属 最 为 经 典 的 一 种 。 它 充分 考虑 了 在 图 像 变 换 过 程 中 出 现 的 光照 、 尺 
有 度 、 旋 转 等 变化 ， 但 随 之 而 来 的 是 极 大 的 计算 量 。 由 于 整个 SLAM 过 程 
中 图 像 特 征 的 提取 与 匹配 仅仅 是 诸 多 环 市 中 的 一 个 ， 到 目前 (2016 年 ) 
为 止 ， 普 通 PC 的 CPU 还 无 法 实时 地 计算 SIFT 特 征 ， 进 行 定位 与 建 图 。 
所 以 在 SLAM 中 我 们 甚 少 使 用 这 种 “奢侈 ”的 图 像 特征 。 

夯 一 些 特征 ， 则 考虑 适当 降低 精度 和 健壮 性 ， 以 提升 计算 的 速度 。 
例如 ，FAST 关 键 点 属于 计算 特别 快 的 一 种 特征 点 〈 注 意 这 里 “关键 
RIRS, WHEAT) 。 而 ORB (Oriented FAST and Rotated 
BRIEF) 特征 则 是 目前 看 来 非常 具有 代表 性 的 实时 图 像 特征 。 它 改进 了 
FAST 检 测 子 B3 不 共有 方 同性 的 问题 ， 并 采用 速度 极 局 的 二 进 制 摘 述 子 
BRIEF! ， 使 整个 图 像 特征 提取 的 环节 大 大 加 速 。 根 据 作者 在 论文 中 所 
述 测 试 ， 在 同一 幅 图 像 中 同时 提取 约 1000 个 特征 点 的 情况 下 ，ORB 约 要 


花费 15.3ms，SURF 约 花费 217.3ms，SIFT 约 花费 5228.7ms。 由 此 可 以 看 
出 ，ORB 在 保持 了 特征 子 具 有 旋转 、 尺 度 不 变性 的 同时 ， 速 度 方面 提升 
明显 ， 对 于 实时 性 要 求 很 高 的 SLAM 来 说 是 一 个 很 好 的 选择 。 

大 部 分 特征 提取 都 具有 较 好 的 并 行 性 ， 可 以 通过 GPU 等 设备 来 加 速 
计算 。 经 过 GPU 加 速 后 的 SIFT， 就 可 以 满足 实时 计算 要 求 。 但 是 ， 引 入 
GPU 将 带 来 整个 SLAM 成 本 的 提升 。 由 此 带 来 的 性 能 提升 是 否 足 以 抵 去 
付出 的 计算 成 本 ， 需 要 系统 的 设计 人 员 仔 细 考 量 。 在 目前 的 SLAM 方 案 
中 ，ORB 是 质量 与 性 能 之 则 较 好 的 折 中 ， 因 此 ， 我 们 以 ORB 为 代表 介绍 
提取 特征 的 整个 过 程 。 


7.1.2 ORB 特征 


ORB 特 征 亦 由 关键 点 和 摘 述 子 ”两 部 分 组 成 。 它 的 关键 点 称 
为 “Oriented FAST”， 是 一 种 改进 的 FAST 角 点 ， 关 于 什么 是 FAST 角 点 我 
们 将 在 下 文 介绍 。 它 的 描述 子 称 为 BRIEF (Binary Robust Independent 
Elementary Feature) 。 因 此 ， 提 取 ORB 特 征 分 为 如 下 两 个 步 又 : 


1.FAST 角 点 提取 : 找 出 图 像 中 的 “和 角 点 ”。 相 较 于 原版 的 FAST， 
ORB 中 计算 了 特征 点 的 主 方向 ， 为 后 续 的 BRIEF 摘 述 子 增加 了 旋转 不 变 
特性 。 

2.BRIEF 描 述 子 : 对 前 一 步 提 取出 特征 点 的 周转 图像 区 域 进行 摘 

下 面 分 别 介 绍 FAST 和 BRIEF。 

FAST 关 键 点 

FAST 是 一 种 角 点 ， 主 要 检测 局 部 像素 灰 度 变化 明显 的 地 方 ， 以 速 
ERER. CWA: 如 果 一 个 像 际 与 邻 域 的 像素 关 别 较 大 〈 过 亮 或 
Whe) ， 那 么 它 更 可 能 是 角 点 。 相 比 于 其 他 角 点 检测 算法 ，FAST 只 需 
比较 像素 亮度 的 大 小 ， 十 分 快捷 。 它 的 检测 过 程 如 下 ( 见 图 7-2) : 


1. 在 图 像 中 选取 像素 p ， 假 设 它 的 亮度 为 L . 


2. B— T BET CHEM, r 872090) 。 


3. 以 像素 p 为 中 心 ， 选 取 半 径 为 3 的 圆 上 的 16 个 像素 点 。 
4. 假 如 选取 的 圆 上 有 连续 的 N 个 点 的 亮度 大 于 I, +T 或 小 于 I -T, AB 


么 像素 p 可 以 被 认为 是 特征 点 CN 通常 取 12， 即 为 FAST-12。 其 他 常用 
的 N 取 值 为 9 和 11， 它 们 分 别 被 称 为 FAST-9 和 FAST-11) 。 


5. 循 环 以 上 四 步 ， 对 每 一 个 像 系 执行 相同 的 操作 。 


在 FAST-12 算 法 中 ， 为 了 更 局 效 ， 可 以 添加 一 项 预测 试 操作 ， 以 快 

速 地 排除 绝 大 多 数 不 是 角 点 的 像 系 。 其 体操 作为 ， 对 于 每 个 像 系 ， 直 接 
检测 邻 域 加 上 的 第 1,5,9,13 个 像 系 的 膨 度 。 只 有 当 这 4 个 像 系 中 有 3 个 同 
HEFL +T 或 小 于 I -T 时 ， 当 前 像素 才 有 可 能 是 一 个 角 点 ， 藻 则 应 该 
直接 排除 。 这 样 的 预测 试 操 作 大 大 加 速 了 角 扣 检测。 此 外 ， 原 始 的 
FAST 角 点 经 常 出 现 “ 扎 堆 ” 的 现象 。 所 以 在 第 一 过 检 疯 之 后 ， 还 需要 用 
非 极 大 值 抑制 (Non-maximal suppression)〉， 在 一 定 区 域内 仪 保留 啊 应 
极 大 值 的 角 点 ， 避 人 免 角 点 集中 的 问题 。 





图 7-2 FASTE. 。 


FASTER MITRANE ERAR HREN, RREA, 
但 它 也 有 一 些 问 题 。 首 先 ，FAST 特 征 点 数量 很 大 且 不 确定 ， 而 我 们 往 
往 希 望 对 图 像 提 取 固 定数 量 的 特征 。 因 此 ， 在 ORB 中 对 原始 的 FAST 算 
法 进行 了 改进 。 我 们 可 以 指定 最 终 要 提取 的 角 点 数量 N ， 对 原始 FAST 
角 点 分 别 计算 Harris 啊 应 值 ， 然 后 选取 前 N 个 具有 最 大 啊 应 值 的 角 点 作 


为 最 终 的 角 扣 集合。 

其 次 ，FAST 角 点 不 具有 方 同 信息 。 而 且 ， 由 于 它 固 定 取 半径 为 3 的 
H, ATER: wehbe eRe ANH, Bata Al pew he A 
点 了 。 针 对 FAST 角 点 不 具有 方向 性 和 尺度 的 弱点 ，ORB 添 加 了 尺度 和 
旋转 的 接 述 。 尺 度 不 变性 由 构建 图 像 金字 培 趾 ， 并 在 金字 菩 的 每 一 层 上 
检测 角 点 来 实现 。 而 特征 的 旋转 是 由 灰 虚 质心 法 (Intensity Centroid) 
实现 的 。 我 们 稍微 介绍 一 下 。 

所 谓 质心 是 指 以 网 像 块 灰 度 值 作为 权重 的 中 心 。 其 具体 操作 步 又 如 
下 [35] ， 


1. 在 一 个 小 的 图 像 块 B F, EBRR N 
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3. 连 接 图 像 块 的 几何 中 心 0 BOC, IADE, To 
特征 点 的 方 癌 可 以 定义 为 
0 = arctan(mo; /m10). 
退 过 以 上 方法 ，FAST 角 点 便 上 共有 了 尺度 与 旋转 的 手 述 ， 从 而 大 大 


提升 了 其 在 不 同 图 像 之 间 表 述 的 健壮 性 。 所 以 在 ORB 中 ， 把 这 种 改进 后 
的 FAST 称 为 Oriented FAST. 


BRIEF 摘 述 子 

在 提取 Oriented FAST 关 键 点 后 ， 我 们 对 每 个 点 计算 其 摘 述 子 。 
ORB 使 用 改进 的 BRIEF 特 征 描述 。 我 们 先 来 介绍 一 下 BRIEF 是 什么 。 

BRIEF 是 一 种 二 进 制 描述 和子 ， 其 摘 述 辐 量 由 许多 个 0 和 1 组 成 ， 这 里 
的 0 和 1 编码 了 关键 点 附近 两 个 像素 〈 比 如 p Ma ) 的 大 小 关系 : 如 果 p 
lig 大 ， 则 取 1， 反 之 就 取 0。 如 果 我 们 取 了 128 个 这 样 的 p,q ， 最 后 就 得 


到 128 维 由 0、1 组 成 的 向 量 。 那 么 ，p Ma 如 何 选 取 呢 ? 在 作者 原始 的 论 
文中 给 出 了 若干 种 挑选 方法 ， 大 体 上 都 是 按照 某 种 概率 分 布 ， 随 机 地 挑 
选 p 和 g 的 位 置 ， 读 者 可 以 阅读 BRIEF 论 文 或 OpenCV 源 码 以 查看 其 具体 
KBA 。BRIEF 使 用 了 随机 选 点 的 比较 ， 速 度 非常 快 ， 而 且 由 于 使 用 了 
二 进 制 表达 ， 存 储 起 来 也 十 分 方便 ， 适 用 于 实时 的 图 像 岂 配 。 原 始 的 
BRIEF 描 述 子 不 具有 旋转 不 变性 ， 因 此 在 图 像 发 生 旋转 时 容易 丢失 。 而 
ORB 在 FAST 特 征 点 提取 阶段 计算 了 关键 点 的 方 同 ， 所 以 可 以 利用 方 回 
信息 ， 计 算 了 旋转 之 后 的 “Steer BRIEF” 特 征 使 ORB 的 描述 子 具 有 较 好 的 
旋转 不 变性 。 

由 于 考虑 到 了 旋转 和 缩放 ， 使 得 ORB 在 平移 、 旋 转 和 缩放 的 变换 下 
仍 有 良好 的 表现 。 同 时 ，FAST 和 BREIF 的 组 合 也 非常 高 效 ， 使 得 ORB 
特征 在 实时 SLAM 中 非常 受 欢迎 。 我 们 在 图 7-3 中 展示 了 一 张 图 像 提 取 
ORB 之 后 的 结果 ， 下 面 来 介绍 如 何在 不 同 的 图 像 之 则 进行 特征 匹配 。 
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图 7-3 ”OpenCV 提 供 的 ORB 特 征 点 检测 结果 。 


7.1.3 ”特征 匹配 


特征 匹配 〈 如 图 7-4 所 示 ) 是 视觉 SLAM 中 极为 关键 的 一 步 ， 宽 泛 地 
说 ， 特 征 匹 配 解决 了 SLAM 中 的 数据 关联 问题 (dataassociation ) ， 即 确 
定 当 前 看 到 的 路 标 与 之 前 看 到 的 路 标 之 间 的 对 应 关系 。 通 过 对 图 像 与 图 
像 或 者 图 像 与 地 图 之 间 的 摘 述 子 进行 准确 匹配 ， 我 们 可 以 为 后 续 的 姿态 
估计 、 优 化 等 操作 减轻 大 量 负 担 。 然 而 ， 由 于 图 像 特 征 的 局 部 特性 ， 误 
匹配 的 情况 广泛 存在 ， 而 且 长 期 以 来 一 下 没有 得 到 有 效 解决 ， 目 前 已 经 
成 为 视觉 SLAM 中 制约 性 能 提升 的 一 大 短 氏 。 部 分 原因 是 场景 中 经 部 存 
在 大 量 的 重复 纹理 ， 使 得 特征 摘 述 非常 相似 。 在 这 种 情况 下 ， 仅 利用 局 
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图 7-4 两 帧 图 像 间 的 特征 匹配 。 


不 过 ， 让 我 们 先 来 看 正确 匹配 的 情况 ， 等 做 完 实 验 再 回头 去 讨论 误 
史 配 问题 。 考 虑 两 个 时 刻 的 图 像 。 如 果 在 图 像 T 中 提取 到 特征 点 
zm,m 二 1,2,.…M， 在 图 像 1I ,， 中 提取 到 特征 点 zB1,n=1,2,…,N， 如 何 寻 找 
这 两 个 集合 元 素 的 对 应 关系 呢 ? 最 简单 的 特征 匹配 方法 惑 是 暴力 匹配 C 
Brute-Force Matcher) 。 即 对 每 一 个 特征 点 吕 与 所 有 的 zx 测量 描述 子 
的 距离 ， 然 后 排序 ， 取 最 近 的 一 个 作为 囊 配 点 。 描 述 子 距离 表示 了 两 个 
特征 之 间 的 相似 程度 ”， 不 过 在 实际 运用 中 还 可 以 取 不 同 的 距离 上 度量 范 
数 。 对 于 浮 点 类 型 的 描述 子 ， 使 用 欧 氏 距离 进行 度量 即 可 。 而 对 于 二 进 
制 的 描述 和子“ 比如 BRIEF 这 样 的 ) ， 我 们 往往 使 用 汉 明 距离 (Hamming 
distance) 作为 上 度量 一 一 两 个 二 进 制 串 之 则 的 汉 明 距离 ， 指 的 是 其 不 同 
位 数 的 个 数 。 

然而 ， 当 特征 点 数量 很 大 时 ， 姑 力 匹配 法 的 运算 量 将 变 得 很 大 ， 特 
别 是 当 想 要 匹配 某 个 帧 和 一 张 地 图 的 时 候 。 这 不 符合 我 们 在 SLAM 中 的 
实时 性 需求 。 此 时 快速 近似 最 近邻 FLANN) 算法 更 加 适合 于 匹配 点 
数量 极 多 的 情况 。 由 于 这 些 匹 配 算法 理论 已 经 成 熟 ， 而 且 实 现 上 也 已 集 


成 到 OpenCV， 所 以 这 里 就 不 再 描述 它 的 技术 细节 了 。 感 兴趣 的 读者 可 
以 参考 阅 计 文献 [36]。 


7.2 ”实践 : 特征 提取 和 匹配 





图 7-5 ”实验 使 用 的 两 帧 图 像 。 


日 前 主流 的 几 种 图 像 特征 在 OpenCV 开 源 图 像 库 中 都 已 经 集成 ， 我 
们 可 以 很 方便 地 进行 调用 。 下 面 束 来 实际 练习 一 下 OpenCV 的 图 像 特 征 
提取 、 计 算 和 逻 配 的 过 程 。 我 们 为 此 实验 准备 了 两 张 图 像 ， 位 于 
slambook/ch7/ 下 的 1.png 和 2.png， 如 图 7-5 所 示 。 它 们 是 来 目 公 开 数 据 集 
[37] 中 的 两 张 图 像 ， 我 们 看 到 相机 发 生 了 人 微小 的 运动 。 本 节 程 序 演示 如 
何 提 取 ORB 特 人 征 并 进行 还 配 。 下 一 个 程序 将 演示 如 何 估 计 相 机 运动 。 


特征 捉 取 与 匹配 代 但 : 


leh) slambook/ch7/feature_extraction.cpp 





#include <iostream> 


#include <opencv2/core/core.hpp> 
#include <opencv2/features2d/features2d.hpp> 
#include <opencv2/highgui/highgui .hpp> 


using namespace std; 


using namespace cv; 


int main ( int argc, char** argv ) 


{ 
if ( argo I= 3 2 
1 
cout<<"usage: feature extraction imgl img2"««endl; 
return 1; 
} 


//-- 读 取 图 像 


Mat img 1 = imread ( argv[i], CV LOAD IMAGE COLOR ); 
Mat img 2 - imread ( argv[2], CV LOAD IMAGE COLOR ); 


//-- 初始 化 

std::vector<KeyPoint> keypoints 1, keypoints 2; 

Mat descriptors 1, descriptors 2; 

Ptr<ORB> orb = ORB::create ( 500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE,31,20 ); 


//-- 第 1 步 : 检测 Oriented FAST 角 点 位 置 
orb->detect ( img 1i,keypoints 1 ); 
orb-»detect ( img 2,keypoints 2 ); 


//-- 第 2 步 : 根据 角 点 位 置 计算 BRIEF 描述 子 
orb-»compute ( img 1, keypoints 1, descriptors 1 ); 


orb->compute ( img 2, keypoints 2, descriptors 2 ); 


Mat outimgi; 

drawKeypoints( img 1, keypoints 1, outimgi, Scalar::all(-1), DrawMatchesFlags:: 
DEFAULT ); 

imshow ("ORB 4¥ 4t 5. ",outimgi); 


//-- 第 3 步 : 对 两 幅 图 像 中 的 BRTEF 描 述 子 进行 匹配 ， 使 用 Hamming 距离 
Vector<DMatch> matches ; 
BFMatcher matcher ( NORM HAMMING ); 


matcher.match ( descriptors 1, descriptors 2, matches ); 


//-- 第 4 步 : 匹配 点 对 筛选 
double min dist-10000, max dist-0; 


// 找 出 所 有 匹配 之 间 的 最 小 距离 和 最 大 距离 ， 即 最 相似 的 和 最 不 相似 的 两 组 点 之 间 的 距离 
for ( int i = 0; i < descriptors 1.rows; i++ ) 
1 

double dist = matches[i].distance; 

if ( dist < min dist ) min dist = dist; 


if ( dist > max dist ) max dist = dist; 


printf ( "-- Max dist : 4f Mn", max dist ); 
printf ( "== Min dist : Xf Xn", min dist ); 


// 当 描 述 子 之 间 的 距离 大 于 两 倍 的 最 小 距离 时 ， 即 认为 匹配 有 误 
// 但 有 时 候 最 小 距离 会 非常 小 ， 设 置 一 个 经 验 值 作为 下 限 。 

std: :Vector< DMatch > good matches; 

for ( int i = 0; i < descriptors 1.rows; i++ ) 


1 


61 if ( matches[il.distance <= max ( 2*min dist, 30.0 ) ) 


62 

63 good matches.push back ( matches[i] ); 

64 } 

65 } 

66 

67 //-- 第 5 步 : 绘制 匹配 结果 

68 Mat img match; 

69 Mat img goodmatch; 

70 drawMatches ( img 1, keypoints 1, img 2, keypoints 2, matches, img match ); 

71 drawMatches ( img 1, keypoints 1, img 2, keypoints 2, good matches, 
img goodmatch ); 

72 imshow ( "所 有 匹配 点 对 "，img_match ); 

73 imshow ( "优化 后 匹配 点 对 "，img_goodmatch ); 

74 waitKey (0) ; 

75 

76 return 0; 

7 |} 


运行 此 程序 需要 输入 两 个 图 像 位 置 ) ， 将 输出 运行 结 







1 |% build/feature extraction 1.png 2.png 
2 |-- Max dist : 95.000000 
3 |-- Min dist : 4.000000 
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一 种 工程 上 的 经 验方 法 ， 不 一 定 有 理论 依据 。 不 过 ， 尽 管 在 示例 图 像 中 
能 够 筛选 出 正确 的 匹配 ， 但 我 们 仍然 不 能 保证 在 所 有 其 他 图 像 中 得 到 的 
匹配 都 是 正确 的 。 因 此 ， 在 后 面 的 运动 估计 中 ， 还 需要 使 用 去 除 误 匹配 
的 算法 。 


接 下 来 ， 我 们 希望 根据 匹配 的 点 对 估计 相机 的 运动 。 这 里 由 于 相机 
的 原理 不 同 ， 人 情况 友 生 了 变化 : 


1. 当 相机 为 单 目 时 ， 我 们 只 知道 2D 的 像素 坐标 ， 因 而 问题 是 根据 两 
组 2D 点 估计 运动 。 该 问题 用 对 极 几 何 来 解决 。 

2. 当 相机 为 双 目 、RGB-D 时 ， 或 者 通过 某 种 方法 得 到 了 距离 信息 ， 
那么 问题 就 是 根据 两 组 3D 点 估计 运动 。 该 问题 通常 用 ICP 来 解决 。 

3. 如 果 有 3D 点 及 其 在 相机 的 投影 位 置 ， 也 能 估计 相机 的 运动 。 该 问 
题 通 过 PnP 求解 。 

内 此， 下 面 几 市 就 来 介绍 这 三 种 情形 下 的 相机 运动 估计 。 我 们 将 从 


最 基本 的 2D-2D 情 形 出 友 ， 看 看 它 如 何 求解 ， 求 解 过 程 义 共有 哪些 托 糯 
的 问题 。 


ORB 关 键 点 





图 7-6 ”特征 提取 与 匹配 结果 。 


7.3 2D-2D: 对 极 几 何 


7.3.1 ”对 极 约束 


现在 ， 假 设 我 们 从 两 张 图 像 中 得 到 了 一 对 配对 好 的 特征 点 ， 如 图 7- 
7 所 示 。 如 来 有 右 干 对 这 样 的 逻 配 扣 ， 束 可 以 通过 这 些 二 维 图 像 点 的 对 
应 关系， 恢复 出 在 两 帧 之 间 摄 像 机 的 运动 。 这 里 “ 奢 干 对 ”具体 是 多 少 对 
Ne? 我 们 会 在 下 文 介绍 。 下 面 移 来 看 看 两 个 图 像 当 中 的 匹配 点 有 什么 几 
何 天 系 。 





图 7-7 对 极 几 何 约束 。 


以 图 7-7 为 例 ， 我 们 希望 求 取 两 帧 图 像 1 ,1 ,之 间 的 运动 ， 设 第 一 帧 3 
第 二 帧 的 运动 为 R,t 。 两 个 相机 中 心 分 别 为 0 ,,O ,。 现 在 ， 考 虑 I ,中 有 - 
个 特征 点 p > EEI, 中 对 应 看 特征 点 P，。 我 们 知道 两 者 是 通过 特征 匹 本 
得 到 的 。 如 果 匹 配 正 确 ， 说 明 它 们 确实 是 同一 个 空间 点 在 两 个 成 像 平 
面 上 的 投影 ”。 这 里 需要 一 些 术语 来 摘 述 它们 之 间 的 几何 关系 。 首 移 ， 
JEL Op, MER Op 在 三 维 空间 中 会 相交 于 点 P 。 这 时 候 点 0 ,,O,,P = 
个 点 可 以 确定 一 个 平面 ， 称 为 极 平 面 〈《Epipolar plane) . OO, #25 
(PT I, WZATI Ne, e,o e,,e, BRAR CEpipole) , O,O 
为 基线 (Baseline) 。 我 们 称 极 平 面 与 两 个 像 平 面 a 之 间 的 相交 线 1 


为 极 线 ( Epipolar line) . 

HMH, Mea WEN ARE, Wop — ced MA HI Beda BUT] 28 
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Opi 在 第 二 个 相机 中 的 投影 。 现 在 ， 由 于 我 们 通过 特征 点 匹配 确定 了 p . 
的 像 系 位 置 ， 所 以 能 够 推 革 PP 的 空间 位 置 ， 以 及 相机 的 运动 。 要 提醒 读 
者 的 是 ， 这 完全 多 亏 了 正确 的 特征 匹配 。 如 果 没 有 特征 还 配 ， 我 们 束 
没 法 确定 p , a 到底 在 极 线 的 哪个 位 置 了 。 那 时 ， 束 必须 在 极 线 上 搜索 以 获 
得 正确 的 思 配 ， 这 将 在 第 13 讲 中 提 人 到 。 

现在 ， 我 们 从 代数 角度 来 看 一 下 这 里 出 现 的 几何 关系 。 在 第 一 帧 的 
坐标 系 下 ， 设 P 的 空间 位 置 为 


P = |X,Y,Z]". 


根据 第 5 讲 介绍 的 针 孔 相机 模型 ， 我 们 知道 两 个 像素 点 p , ,p , 的 像素 
位 置 为 
S1D1 = KP, Sg p2 = K (RP 十 t) š (7.1) 
这 里 K ZJBELEISSBEE, Rt 为 两 个 坐标 系 的 相机 运动 〈 如 采 我 们 
愿意 ， 也 可 以 写成 字 代 数 形式 ) 。 如 果 使 用 齐 次 坐标 ， 也 可 以 把 上 式 与 
TE FE CASES BCR OLA Cup to a scale) SEX"! : 


pi=KP, po=K(RP +t). (7.2) 
现在 ， HY: 
xı = Kp, x£= K p. (7.3) 


这 里 的 x 1 ,x ,十 两 个 像 系 扣 的 归 一 化 平面 上 的 坐标 。 代 入 上 式 ， 得 : 
qo = Ra, Ta. (7.4) 


PHI TAY Ac Fee’ o EHZ ^ 的 定义 ， 这 相当 于 两 侧 同时 与 ! 做 外 积 : 


tao = t^ Rz. (7.5) 
IA PAEA 35625: 
Tt zx2 = xi t^ RTI. (7.6) 
MZEE, ^x, 是 一 个 与 t 和 x ,都 垂直 的 同 量 。 把 它 再 和 x ,做 
内 积 时 ， 将 得 到 0。 因 此 ， 我 们 就 得 到 了 一 个 简 涪 的 式 子 : 
zt Ra, = 0. (7.7) 
重新 代入 p , Dos A: 
pK t^RK !p, = 0. (7.8) 


这 两 个 式 子 都 称 为 对 极 约 束 . EUER EA. UBL xs X. 
720 , ,P,O ,三 者 共 面 。 对 极 约 束 中 同时 包含 了 平移 和 旋转 。 我 们 把 中 间 
部 分 记 作 两 个 矩阵 ;基础 矩阵 (Fundamental Matrix) 正和 本 质 和 矩阵 
(Essential Matrix) E, ， 于 是 可 以 进一步 简化 对 极 约束 : 


E=t^R, F—-K EK |, 2x,Ex, = pl Fp =0. (7.9) 

对 极 约束 人 窗 涪 地 给 出 了 两 个 匹配 点 的 空间 位 置 关 系 。 于 是 ， 相 机 位 
次 估计 问题 变 为 以 下 两 步 : 

1. 根 据 配对 点 的 像素 位 置 求 出 五 或 者 F o 

2. 根 据 E 或 者 已 求 出 Rit 。 

由 于 E MF 只 相差 了 相机 内 参 ， 而 内 参 在 SLAM 中 通常 是 已 知 的 名 
， 有 所 以 实践 当中 往往 使 用 形式 更 简单 的 E 。 我 们 以 E 为 例 ， 介 绍 上 面 两 
个 问题 如 何 求解 。 
7.3.2 kJ BE 


根据 定义 ， 本 质 和 矩阵 E =t^ R 。 它 是 一 个 3x 3 的 矩阵， 内 有 9 个 未 知 


数 。 那 么 ， 是 不 是 任意 一 个 3x 3 的 窍 阵 都 可 以 被 当 成 本 质 窍 阵 呢 ? ME 
的 构造 方式 上 看 ， 有 以 下 值得 注意 的 地 方 : 

本 质 滤 阵 是 由 对 极 约束 定义 的 。 由 于 对 极 约束 是 等 式 为 和 零 。 ”的 约 
R, MUWE 乘 以 任意 非 零 凋 数 后 ， 对 极 约束 依然 满足 。 我 们 把 这 件 
事情 称 为 正在 不 同 尺 度 下 是 等 价 的 。 

RHE t^ R, PJRED ， 本 质 窍 阵 下 的 奇异 值 必定 是 [oa, 0]* 的 
形式 。 这 称 为 本 质 窍 阵 的 内 在 性 质 。 

* 必 一 方面 ， 由 于 平移 和 旋转 各 有 3 个 目 由 度 ， 故 忆 R 共有 6 个 目 由 
BE. (HEHT RESUME, BE SEES LAST BL EHE. 

E RAST AAS, KHR n] DAS ARKE 。 但 
是 ,五 INS AEP PIERE, TES ARE DT FER] 2 TH RL 
因此 ， 也 可 以 只 考虑 它 的 尺度 等 价 性 ， 使 用 8 对 点 来 估计 下 一 一 这 就 古 
经 典 的 八 点 法 CEight-point-algorithm) [829] 。 八 点 法 只 利用 了 E 的 线性 
性 质 ， 因 此 可 以 在 线性 代数 框架 下 求解 。 下 面 我 们 来 看 八 点 法 是 如 何 工 
TER « 

考虑 一 对 匹配 点 ， 它 们 的 归 一 化 坐标 为 x; 7[u ,,v ,, AT. ,x ,=[u P v S, 
。 根 据 对 极 约束 ， 有 : 


C1 €2 €3 u2 
(ui 0,1) €4 €s ef Vo | = 0. (7.10) 
€7 eg eg 1 


我 们 把 矩阵 E 展开 ， 写 成 同 量 的 形式 : 
& = [£1,82, 65,64, 65, 65, 67, 68, Ga]; 
那么 对 极 约束 可 以 写成 与 e 有 关 的 线性 形式 : 
[u1u, ui V2, U1, V1U2, U102, V1, U2, V2, 1] - e = 0. (7.11) 


间 理 ， 对 于 其 他 点 对 也 有 相同 的 表示 。 我 们 把 所 有 点 都 放 到 一 个 方 


程 中 ， 变 成 线性 方程 组 《开关 表示 第 i 个 特征 点， 以 此 类 推 ) : 


Gl 
C2 
lod xg b gal aa i gat uf Á 
irus ujus ur iu UU © U5 Vy 1l 
C4 
2 2 D wit of 2 wm oe 
utu) uiv uj viu UU vi ug vg 1 
: : €5 E. PARN 
€6 
8 8 piya pans 48 gè £48 
Uus uqU5 Uz, v Uw v U5 Vy l 
e7 
C8 
C9 


这 8 个 方程 构成 了 一 个 线性 方程 组 。 它 的 系数 矩阵 由 特征 点 位 置 构 
成 ， 大 小 为 8x* 9。e 位 于 该 窍 阵 的 零 空 间 中 。 如 泉 系数 窃 阵 是 满 秩 的 
CIRIB) ， 那 么 它 的 零 空间 维 数 为 1， 也 融 是 e 构成 一 条 线 。 这 与 e 
的 矿 度 等 价 性 是 一 致电 。 如 条 8 对 匹配 点 组 成 的 定 阵 满足 秩 为 8 的 条 件 ， 
A AE 的 各 元 系 驶 可 由 上 述 方程 解 得 。 

Be POR AY Ie) dle SR AAPA EEE ， 恢 复出 相机 的 运 
动 R,t 。 这 个 过 程 是 由 奇异 值 分 解 SVD) 得 到 的 。 设 的 SVD 分 解 为 


E-—UXV!, (7.13) 


其 中 UV WIEZIE, LIATHA., WAE 的 内 在 性 质 ， 我 们 知 
i&X-diag(o,, 0)。 在 SVD 分 解 中 ， 对 于 任意 一 个 E ， 存 在 两 个 可 能 的 6R 
与 它 对 应 : 

t^ -URz(Z)EU,, R,-URL(Z)V! 
th -URz(-Z2)EU!,, R,-URL(-Z)V!. 


(7.14) 


其 中 Rz(3) 表示 治 Z 轴 旋 转 90“ 得 到 的 旋转 矩阵 。 同 时 ， 由 于 -E IE 


等 价 ， 所 以 对 任意 一 个 t 取 负 号 ， 也 会 得 到 同样 的 结 末 。 因 此 ， 从 E 分 
解 到 t,R 时 ， 一 共存 在 4 个 可 能 的 解 。 


图 7-8 形 象 地 展示 了 分 解 本 质 滤 阵 得 到 的 4 个 解 。 我 们 已 知 空间 扣 在 
HIL CEER) 上 的 投影 (红色 点 ) ， 想 要 求解 相机 的 运动 。 在 保持 红 
色 扣 不 变 的 情况 下 ， 可 以 男 出 4 种 可 能 的 情况 。 不 过 辛 运 的 是 ， 只 有 第 
一 种 解 中 P 在 两 个 相机 中 痢 上 共有 正 的 深度 。 因 此 ， 只 要 把 任 童 一 扣 代 入 
4 种 解 中 ， 检 测 该 氮 在 两 个 相机 下 的 深度 ， 惑 可 以 确定 哪个 解 是 正确 的 
f: 





(1) (2) (3) (4) 


图 7-8 MERMERE. ETRE AAR 不 变 的 情况 下 ， 两 个 相机 及 
空间 点 一 共有 4 种 可 能 的 情况 。 


如 果 利 用 E ”的 内 在 性 质 ， 那 么 它 只 有 5 个 自由 度 。 所 以 最 小 可 以 通 
过 5 对 点 来 求解 相机 运动 % 和 入” 。 然 而 这 种 做 法 形式 复杂 ， 从 工程 实现 角 
度 考虑 ， 由 于 平时 通常 会 有 儿 十 对 乃至 上 百 对 的 匹配 点 ， 从 8 对 减 至 5 对 
意义 并 不 明显 。 为 保持 人 简单， 我 们 这 里 束 只 介绍 基本 的 八 点 法 。 

剩 下 的 问题 还 有 一 个 : 根据 线性 方程 解 出 的 E ， 可 能 不 满足 E 的 内 
在 性 质 一 一 它 的 奇异 值 不 一 定 为 0,0， 0 的 形式 。 这 时 ， 在 做 SVD 时 我 们 
会 刻意 地 把 > 窍 阵 调整 成 上 面 的 样子 。 通 常 的 做 法 是 ， 对 八 点 法 求 得 的 E 
进行 SVD 分 解 后 ， 会 得 到 奇异 信 窍 阵 2=diag(ao ,0 , ,0 3 )， 不 妨 设 ai >a，: 
, AM: 





01 Tt s Gi T Fa 
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这 相当 于 是 把 求 出 来 的 矩阵 投影 到 了 E MERRE. 5%, Efi 


E = Udiag( Dj (7.15) 


单 的 做 法 是 将 奇异 值 矩 阵 取 成 diag(1, 1, 0)， 因 为 E 具有 尺度 等 价 性 ， 所 
以 这 样 做 也 是 合 理 的 。 


7.3.3 "EJ AB E 


除了 基本 和 窍 阵 和 本 质 窍 阵 ， 还 有 单 应 窍 阵 〈Homography) H , © 

摘 述 了 两 个 平面 之 间 的 映射 天 系 。 大 场景 中 的 特征 点 都 沙 在 同一 平面 上 

(比如 播 、 地 面 等 ) ， 则 可 以 通过 单 应 性 来 进行 运动 估计。 这 种 情况 在 

无 人 机 携带 的 傅 视 相机 或 扫地 机 携带 的 项 视 相 机 中 比较 彰 抑 。 由 于 之 前 
没有 提 到 过 单 应 ， 因 此 这 里 稍微 介绍 一 下 。 

单 应 窍 阵 通 稼 描述 处 于 共同 平面 上 的 一 些 点 在 两 张 图 像 乙 间 的 变换 

关系 。 考 虑 在 图 像 六 MI, 有 一 对 匹配 好 的 特征 点 P， Mp., 。 这 些 特征 点 


在 平面 P 上 ， 设 这 个 平面 满足 方程 : 





n'P+d=0. (7.16) 
n! P 
ir mm 1. (7.17) 


于 是 ， 我 们 得 到 了 一 个 直接 摘 述 图 像 坐标 p， Mp, 之 间 的 变换 ， 把 中 
由 这 部 分 记 为 H , 于 是 : 


p2 = Hp. (7.18) 


它 的 定义 与 旋转 、 平 移 及 平面 的 参数 有 关 。 与 基础 矩阵 已 类 似 ， 单 
应 窍 阵 五 也 是 一 个 3x 3 的 和 矩阵， 求解 时 的 思路 也 和 下 类似， 同样 可 以 先 
根据 匹配 点 计算 五 ” ， 然 后 将 它 分 解 以 计算 旋转 和 平移。 把 上 和 式 展开 ， 


得 : 


U2 hy ho h3 Ul 
V2 = ha hs hg U1 . (7.19) 
1 hz hg hg 1 


请 注意 ， 这 里 的 等 亏 是 在 非 零 因 了 于 下 成 立 的 。 我 们 在 实际 处 理 中 通 
种 乘 以 一 个 非 零 因子 使 得 A 。=1 EERIE) 。 然 后 根据 第 3 行 ， 
AIRES A YT, TEA: 


u hiui dja ho11 ES ha 
RC hzu1 ra hgv1 E ho 
(hau, + hsvi + he 
ae hzu1 sje hgv1 els ho 


整理 得 : 
hiui jr h5v1 E ha - hzu1u» = hgv1u» = U2 
hauy + hzv4 十 hg = hzu4v» 一 hgv4vo = U2. 
OPE — 2H UL WO OG] aie n] DA tee EH PAR CSRS EQ ZDAR, dH 
是 因为 线性 相关 ， 只 取 前 两 个 ) » ， 于 和 古 目 由 度 为 8 的 单 应 定 阵 可 以 通过 4 


对 匹配 特征 点 算出 〈 注 意 ， 这 些 特征 点 不 能 有 三 点 共 线 的 情况 ) ， 即 求 
解 以 下 的 线性 方程 组 ( 当 n ,=0 时 ， 右 侧 为 零 ) ， 


ul vi 1 0 0 0 uu —viu hy üz 
0 0 0 ut vl 1 —uivy 一 OU ho va 
uf vf 1 0 0 0 -uwtwó —viw$ hs us 
0 0 0 ut v? 1 -—u2v$ -—viv$ ha vs 
= : (7.20) 
uj vi 1 0 0 0 uu —vjui hs us 
0 0 0 wj ve 1 uv -—wviv he vU 
uf vf 1 0 0 0 -wu$ —viu$ hz us 
0 0 0 v? ve 1 —-uh$ -vivj hg V5 


BOAT ERAR I lel, TARR Ie) Be AE RK 
， 义 称 直 接线 性 变换 法 (Direct Linear Transform) 。 与 本 质 和 矩阵 相似 ， 
求 出 单 应 算 阵 以 后 需要 对 其 进行 分 解 ， 才 可 以 得 到 相应 的 旋转 窍 阵 R 和 
平移 同 量 t 。 分 解 的 方法 包括 数值 法 中 4 与 解析 法 的 。 与 本 质 官 阵 的 分 
解 类 似 ， 单 应 算 阵 的 分 解 同 样 会 返回 4 组 旋转 窍 阵 与 平移 同 量 ， 并 且 同 
时 可 以 计算 出 它们 分 别 对 应 的 场景 点 所 在 平面 的 法 回 量 。 如 果 已 知 成像 
的 地 图 点 的 深度 全 为 正 值 〈 即 在 相机 前 方 ) ， 则 又 可 以 排除 两 组 解 。 节 
后 仅 剩 两 组 解 ， 这 时 需要 通过 更 多 的 先 验 信息 进行 判断 。 通 钊 我 们 可 以 
通过 假设 已 知 场景 平面 的 法 同 量 来 解决 ， 如 场景 平面 与 相机 平面 平行 ， 
MAE en 的 理论 值 为 17 。 

单 应 性 在 SLAM 中 具有 重要 意义 。 当 特征 点 共 面 或 者 相机 发 生 纯 旋 
转 时 ， 基 础 矩阵 的 目 由 上 度 下 降 ， 这 就 出 现 了 所 请 的 退化 
(degenerate) 。 现 实 中 的 数据 总 包含 一 些 噪声 ， 这 时 候 如 末 继 续 使 用 
八 点 法 求解 基础 窍 阵 ， 基 础 矩阵 多 余 出 来 的 目 由 度 将 会 主要 由 噪声 决 
定 。 为 了 能 够 避免 退化 现象 造成 的 影响 ， 通 第 我 们 会 同时 佑 计 基 础 窍 
EF MEMEH , ， 选 择 重 投影 误差 比 较 小 的 那个 作为 最 终 的 运动 估计 
ARE 


7.4 实践: 对 极 约束 求解 相机 运 双 


下 面 ， 我 们 来 练习 一 下 如 何 通 过 本 质 窍 阵 求解 相机 运动 。 上 一 币 实 
践 部 分 的 程序 提供 了 特征 匹配 ， 而 这 次 我 们 束 使 用 匹配 好 的 特征 点 来 计 
算 E,F 和 五 ， 进 而 分 解 E 得 到 R,t 。 整 个 程序 使 用 OpenCV 提 供 的 算法 进 
行 求解 。 我 们 把 上 一 市 的 特征 提取 封装 成 函数 ， 以 供 后 面 使 用 。 本 节 只 
展示 位 姿 估 计 部 分 的 代码 。 

由 slambook/ch7/pose estimation 2d2d.cpp (Tr EX) 


1 | void pose estimation 2d2d ( 


2 std::vector<KeyPoint> keypoints 1, 

3 std::vector<KeyPoint> keypoints 2, 

4 std::vector< DMatch > matches, 

5 Mat& R, Mat& t ) 

6 [t 

7 // 相机 内 参 ，TUM Freiburg2 

8 Mat K = ( Mat «double» ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); 
9 

10 //-- 把 匹配 点 转换 为 vector«Point2f» 的 形式 

1 vector<Point2f> points1; 

12 vector<Point2f> points2; 

13 

14 for ( int i = 0; i < ( int ) matches.size(); i++ ) 

15 { 

16 pointsi.push back( keypoints 1[matches[i].queryIdx].pt ); 
17 points2.push back( keypoints_2[matches[i].trainIdx].pt ); 
18 } 


20 //-- 计算 基础 矩阵 
21 Mat fundamental matrix; 
22 fundamental matrix = findFundamentalMat ( points1, points2, CV FM 8POINT ); 


23 cout««"fundamental matrix is "<<endl<< fundamental matrix««endl; 


25 //-- FHA BEE 


26 Point2d principal point ( 325.1, 249.7 ); // #8, TUM dataset 标定 值 
27 int focal_length = 521; // &3E, TUM dataset 标定 值 
28 Mat essential_matrix; 

29 essential matrix = findEssentialMat ( points1, points2, focal length, 


principal, point, RANSAC ); 


30 cout««"essential matrix is "<<endl<< essential matrix««endl; 

31 

32 //-- 计算 单 应 矩阵 

33 Mat homography matrix; 

34 homography matrix - findHomography ( pointsi, points2, RANSAC, 3, noArray(), 
2000, 0.99 ); 

35 cout««"homography matrix is "««endl««homography matrix««endl; 


37 //-- 从 本 质 和 矩阵 中 恢复 旋转 和 平移 信息 
38 recoverPose ( essential matrix, pointsi, points2, R, t, focal length, 


principal, point ); 


39 cout<<"R is "««endl««R««endl; 
40 cout<<"t is "««endl««t««endl; 
41 |} 


该 函数 提供 了 从 特征 点 求 解 相 机 运动 的 部 分 ， 然 后 ， 我 们 在 主 函 数 
中 调用 它 ， 束 能 得 到 相机 的 运动 : 
四 slambook/ch7/pose estimation 2d2d.cpp (Fr EX 


int main( int argc, char** argv ) 


1 

if ( argc != 3) 

1 
cout<<"usage: feature extraction Imgl img2"««endl; 
return 1; 

} 

//-- 读 取 图 像 

Mat img 1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 

Mat img 2 = imread ( argv[2], CV LOAD IMAGE COLOR ); 


vector<KeyPoint> keypoints 1, keypoints 2; 
Vector<DMatch> matches; 
find feature matches( img 1, img 2, keypoints 1, keypoints 2, matches ); 


cout««" — + iX, £| f "««matches.size()««"2R It fe 5 "««endl; 


//-- 估计 两 张 图 像 闻 的 运动 
Mat R,t; 
pose estimation 2d2d( keypoints 1, keypoints 2, matches, R, t ); 


20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 


40 


~ 人 
M. 


//-- 验证 E=t Rxscale 

Mat t x = (Mat <double>(3,3) << 
0, -t.at«double»(2,0), t.at<double>(1,0), 
t.at<double>(2,0), p. -t.at«double»(0,0), 
-t.at«double»(1.0), t.at«double»(0,0), DJ : 


cout««"t^R2'"««endl««t x*R««endl; 

//-- 验证 对 极 约束 

Mat K = ( Mat. «double» ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 
for ( DMatch m: matches ) 


1 
Point2d pti = pixel2cam( keypoints 1[ m.queryIdx ].pt, K ); 
Mat y1 = (Mat «double»(3,1) << pti.x, pti.y, 1); 
Point2d pt2 = pixel2cam( keypoints 2[ m.trainIdx ].pt, K ); 
Mat y2 = (Mat «double»(3,1) << pt2.x, pt2.y, 1); 
Mat d = y2.t() * t x * R * yt; 
cout << "epipolar constraint = " << d << endl; 

l. 

return 0; 


我 们 在 函数 中 输出 了 E,F MH 的 数值 ， 然 后 验证 了 对 极 约束 是 个 成 
UL At^ R ME 在 非 零 数 乘 下 等 价 的 事实 。 现 在 ， 调 用 此 程序 即 可 看 


到 输出 结 


i |% build/pose estimation 2d2d 1.png 2.png 

2 |-- Max dist : 95.000000 

3 |-- Min dist : 4.000000 

4 | 一 共 找到 了 79 组 匹配 点 

5 | fundamental matrix is 

6 | [4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386; 
7 | -0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593; 
s |0.01799658210895528, 0.008143605989020664, 1] 

9 | essential matrix is 

10 | [-0.0203618550523477, -0.4007110038118445, -0.03324074249824097; 

1 | 0.3939270778216369, -0.03506401846698079, 0.5857110303721015; 

i2 | -0.006788487241438284, -0.5815434272915686, -0.01438258684486258] 

i3 | homography matrix is 

i4 | [0.9497129583105288, -0.143556453147626, 31.20121878625771; 

i5 |0.04154536627445031, 0.9715568969832015, 5.306887618807696; 

i6 | -2.81813676978796e-05, 4.353702039810921e-05, 1] 

7 |B is 

is | [0.9985961798781875, -0.05169917220143662, 0.01152671359827873; 

i9 | 0.05139607508976055, 0.9983603445075083, 0.02520051547522442; 

20 | -0.01281065954813571, -0.02457271064688495, 0.9996159607036126] 


a |t is 

2 | [-0.8220841067933337 ; 
23 | -0.03269742706405412; 
24 | 0.5684264241053522] 


26 | t R= 

27 | [0.02879601157010516, 0.5666909361828478, 0.04700950886436416; 
28 | -0.5570970160413605, 0.0495880104673049, -0.8283204827837456; 
29 |0.009600370724838804, 0.8224266019846683, 0.02034004937801349] 
30 |epipolar constraint = [0.002528128704106625] 

3 | epipolar constraint = [-0.001663727901710724] 

3 |epipolar constraint = [-0.0008009088410884102] 





在 程序 的 输出 结果 可 以 看 出 ， 对 极 约束 的 满足 精度 约 在 10 ^ ? EE 
级 。 根 据 前 面 的 讨论 ， 分 解 得 到 的 R,t 一 共有 4 种 可 能 性 。 不 过 ， 
OpenCV 会 蔡 我 们 使 用 三 角 化 检测 角 扣 的 深 大 是否 为 正 ， 从 而 选 出 正确 


的 解 。 
南 要 注意 的 地 方 是 ， 我 们 要 卉 清 程序 求解 出 来 的 Rt 征 什 么 意义 。 
按照 例 程 的 定义 ， 我 们 的 对 极 约束 是 从 


to — Ra, TL 


得 到 的 。 这 里 的 Rt HAREE, X698 — 7 Ed uS TAA 
bae ORBE: 


qoo 一 T5121. (7.21) 


请 读者 在 实践 中 务必 清楚 这 里 使 用 的 变换 顺序 〈 因 为 有 时 我 们 会 
用 T，) ， 它 们 非常 容易 搞 反 o 

讨论 

从 演示 程序 中 可 以 看 到 ， 输 出 的 E 和 F 之 间 相 差 了 相机 内 参 算 阵 。 
虽然 它们 在 数值 上 并 不 直观 ， 但 可 以 验证 它们 的 数学 关系 。 从 E,F5 MH 
都 可 以 分 解 出 运动 ， 不 过 万 需要 假设 特征 点 位 于 平面 上 。 对 于 本 实验 的 
数据 ， 这 个 假设 是 不 好 的 ， 所 以 我 们 这 里 主要 用 E 来 分 解 运 动 。 

值得 一 提 的 是 ， 由 于 E 本 有 身 具 有 尺度 等 价 性 ， 它 分 解 得 到 的 6R 也 
AS RESUME. MRE SO(3) 上 自身 具有 约束 ， 所 以 我 们 认为 tf 具有 一 
个 尺度 。 换 言 之 ， 在 分 解 过 程 中 ， 对 + 乘 以 任意 非 零 常数 ， 分 解 都 是 成 
并 的 。 因 此 ， 我 们 通 第 把 t 进行 归 一 化 ， 让 和 它 的 长 上 度 等 于 1。 

尺度 不 确定 性 


对 t 长 度 的 归 一 化 ， 直 接 导 致 了 单 目 视 党 的 尺度 不 硝 定 性 〈 Scale 
Ambiguity) 。 例 如 ， 程 序 中 输出 的 ! 第 一 维 约 为 0.822。 这 个 0.822 究 竟 
是 指 0.822 米 还 是 0.822 厘 米 ， 我 们 是 没 法 确定 的 。 因 为 对 t 乘 以 任意 比例 
常数 后 ， 对 极 约 束 依然 是 成 立 的 。 换 言 之 ， 在 单 日 SLAM 中 ， 对 轨迹 和 
地 图 同时 缩放 任意 倍数 ， 我 们 得 到 的 图 像 依 然 是 一 样 的 。 这 在 第 2 讲 中 
OAL AW J 

在 单 目 视觉 中 ， 我 们 对 两 张 图 像 的 ! 归 一 化 相当 于 固定 了 尺度 。 虽 
然 我 们 不 知道 它 的 实际 长 度 是 多 少 ， 但 我 们 以 这 时 的 t 为 单位 1， 计 算 相 
机 运动 和 特征 点 的 3D 位 置 。 这 被 称 为 单 日 SLAM 的 初始 化 ”。 在 初始 化 


之 后 ， 就 可 以 用 3D-2D 来 计算 相机 运动 了 。 初 始 化 之 后 的 轨迹 和 地 图 的 
单位 ， 就 是 初始 化 时 国定 的 尺度 。 因 此 ， 单 目 SLAM 有 一 步 不 可 避免 的 
初始 化 ”。 初 始 化 的 两 张 图 像 必须 有 一 定 程度 的 平移 ， 而 后 的 轨迹 和 地 
图 都 将 以 此 步 的 平移 为 单位 。 

除了 对 t 进行 归 一 化 之 外 ， 另 一 种 方法 是 令 初 始 化 时 所 有 的 特征 点 
平均 深度 为 1， 也 可 以 固定 一 个 尺度 。 相 比 于 令 ! 长 度 为 1 的 做 法 ， 把 特 
征 点 深度 归 一 化 可 以 控制 场景 的 规模 大 小 ， 使 计算 在 数值 上 更 稳定 些 。 
不 过 这 并 没有 理论 上 的 差别 。 

初始 化 的 纯 旋转 问题 


ME 分 解 到 R,t 的 过 程 中 ， 如 果 相 机 发 生 的 是 纯 放 转 ， 导 致 ! AS, 
那么 ， 得 到 的 E 也 将 为 零 ， 这 将 导致 我 们 无 从 求解 R 。 不 过 ， 此 时 我 们 
可 以 依靠 五 求 取 旋转 ， 但 仅 有 旋转 时 ， 我 们 无 法 用 三 角 测 量 估 计 特 征 点 
的 空间 位 置 (这 将 在 下 文 提 到 ) ， 于 是 ， 男 一 个 结论 是 ， 单 目 初 始 化 不 
能 只 有 纯 旋转 ， 必 须要 有 一 定 程 度 的 平移 、。 如 果 没 有 平移 ， 单 目 将 无 
法 初始 化 。 在 实践 当中 ， 如 果 初 始 化 时 平移 太 小 ， 会 使 得 位 姿 求 解 与 三 
角 化 结果 不 稳定 ， 从 而 导致 失败 。 相 对 地 ， 如 末 把 相机 左右 移动 而 不 是 
原 地 旋转 ， 就 容易 让 早 目 SLAM 和 初始 化 。 因 而 ， 有 经 验 的 SLAM 研 究 人 
员 ， 在 单 目 SLAM 情 况 下 经 常 选择 让 相机 进行 左右 平移 以 顺利 地 进行 初 
始 化 。 

多 于 8 对 点 的 情况 


当 给 定 的 点 数 多 于 8 对 时 (比如 ， 例 程 找到 了 79 对 匹配 ) ， 我 们 可 
以 计算 一 个 最 小 二 乘 解 。 回 忆 式 (7.12) 中 线性 化 后 的 对 极 约 束 ， 我 们 把 
FAME ABE MEIC AA : 


Ae = 0. (7.22) 


WPI, A 的 大 小 为 8x 9。 如 果 给 定 的 四 配点 多 于 8， 访 方程 
构成 一 个 超 定 方程 ， 即 不 一 定 存 在 e 使 得 上 式 成 立 。 因 此 ， 可 以 通过 最 
小 化 一 个 二 次 型 来 求 : 


min 14ell: — min e! A! Ae. (7.23) 
€ € 


于 是 束 求 出 了 在 最 小 二 乘 意 义 下 的 已 FE. Ail, SPR REECE RL 


配 的 情况 时 ， 我 们 会 更 倾 问 于 使 用 随机 采样 一 致 性 ( Random Sample 
Concensus, RANSAC) 来 求 ， 而 不 是 最 小 二 滋 。RANSAC 是 一 种 通用 
的 做 和 法， 适用 于 很 多 计 错误 数据 的 情况 ， 可 以 处 理 带 有 错误 匹配 的 数 
HE o 


7.5 三 角 测 量 


之 前 两 节 我 们 使 用 对 极 几 何 约 束 估 计 了 相机 运动 ， 也 讨论 了 这 种 方 
法 的 局 限 性 。 在 得 到 运动 之 后 ， 下 一 步 我 们 需要 用 相机 的 运动 估计 特征 
点 的 空间 位 置 。 在 蛙 目 SLAM 中 ， 仪 通过 曲张 图 像 无 法 获得 像 双 的 深 谋 
信息 ， 我 们 需要 通过 三 角 测 量 〈 Triangulation) (或 三 角 化 ) 的 方法 来 
估计 地 图 点 的 深度 ， 如 图 7-9 所 示 。 


e 


«IS 


O, 


图 7-9 三 角 化 获得 地 图 点 深度 

三 角 测 量 是 指 ， 通 过 在 两 处 观察 同一 个 点 的 夹 角 ， 从 而 确定 该 点 的 
距离 。 三 角 测 量 最 早 由 局 斯 提出 并 应 用 于 测量 学 中 ， 它 在 天 文学 、 地 理 
学 的 测量 中 都 有 应 用 。 例 如 ， 我 们 可 以 通过 不 同 季 市 观 聚 到 的 星星 的 角 
度 ， 佑 计 它 离 我 们 的 距离 。 在 SLAM 中 ， 我 们 主要 用 三 角 化 来 估计 像素 
点 的 距离 。 

和 上 一 节 类 似 ， 考 虑 图 像 六 和 I , ， 以 左 图 为 参考 ， 右 图 的 变换 矩阵 
AT. 。 相 机 光 心 为 0 ,和 0O ; 。 在 1 中 有 特征 点 p , ， 对 应 7, 中 有 特征 点 p 


。 理论 上 下 线 O pi 与 O ,p ,在 场景 中 会 相交 于 一 后 P ， 该 点 即 两 个 特 自 
所 对 应 的 地 图 点 在 三 维 场景 中 的 位 置 。 然 而 由 于 噪声 的 影响 ， 这 两 条 下 
线 往往 无 法 相交 。 因 此 ， 可 以 通过 最 二 小 乘法 求解 。 

按照 对 极 几 何 中 的 定义 ， 设 x 1 ,x ,为 两 个 特征 点 的 归 一 化 坐标 ， 那 < 
"E AL TW XE: 


S11 一 soRxo + t. (7.24) 
现在 我 们 已 经 知道 了 R,t ， 想 要 求解 的 是 两 个 特征 点 的 深度 s ，,s，。 
当然 这 两 个 深度 是 可 以 分 开 求 的 ， 比 如 ， 先 来 看 s，。 如 末 要 算 s，， 那 么 


先 对 上 式 两 侧 左 乘 一 个 x*^，， 得 : 
5S12 个 21 == sox) Rx» + zt. (7.25) 


该 式 左 侧 为 零 ， 右 侧 可 看 成 s , 的 一 个 方程 ， 可 以 根据 它 直接 求 得 s ， 
CH so s, 也 非常 容易 求 出 。 于 是 ， 我 们 就 得 到 了 两 帧 下 的 点 的 深 
度 ， 确 定 了 它们 的 空间 坐标 。 当 然 ， 由 于 噪声 的 存在 ， 我 们 估 得 的 Rs 
不 一 定 精确 使 式 (7.24) 为 零 ， 所 以 更 常见 的 做 法 是 求 最 小 二 乘 解 而 不 是 
rf. 


AY 


7.6.1 — ff d] m (RH 


下 面 ， 我 们 汗 示 如 何 根据 之 前 利用 对 极 几 何 求解 的 相机 位 姿 ， 通 
三 角 化 求 出 上 一 节 特 征 点 的 空间 位 置 。 我 们 调用 OpenCV 提 供 的 
triangulation K BUT = 4844. 


由 slambook/ch7/triangulation.cpp C Fr Wr) 


1 | void triangulation ( 


2 const vector<KeyPoint>& keypoint 1, 

3 const vector<KeyPoint>& keypoint 2, 

4 const std::vector« DMatch »& matches, 
5 const Mat& R, const Mat& t, 

6 vector<Point3d>& points 

? [3i 


9 |void triangulation 人 


10 const Vector< KeyPoint >& keypoint 1, 

1 const Vector< KeyPoint >& keypoint 2, 

12 const std::vector« DMatch >& matches, 

13 const Mat& R, const Mat& t, 

14 Vector< Point3d >& points ) 

is |t 

16 Mat T1 = (Mat «double» (3,4) << 

17 Lol aM » 

18 0,1,040, 

19 0,0,1,0) i 

20 Mat T2 = (Mat «double» (3,4) 《< 

2] R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at«double»(0,0), 
22 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0), 
23 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0) 


jul 


25 


26 


27 


28 


29 


30 


31 


32 


33 


34 


35 


36 


37 


38 


39 


40 


4] 


42 


43 


44 


45 


46 


47 


48 


49 


50 


同时 , 


Mat K = ( Mat. «double» ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 
vector<Point2d> pts 1, pts 2; 
for ( DMatch m:matches ) 


{ 
// 将 像素 坐标 转换 至 相机 坐标 
pts_1.push_back ( pixel2cam( keypoint 1[m.queryIdx].pt, K) ); 
pts 2.push back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) ); 
) 
Mat pts 4d; 


cv::triangulatePoints( T1, T2, pts 1, pts 2, pts 4d ); 


// 转换 成 非 齐 次 坐标 
for ( int i=0; ixpts 4d.cols; i++ ) 
1 
Mat x = pts 4d.col(i); 
x /= x.at<float>(3,0); // 归 一 化 
Point3d p ( 
x.at<float>(0,0), 
x.at<float>(1,0), 
x.at«float»(2,0) 
); 
points.push_back( p ); 


在 main 函 数 中 增加 三 角 测 量 部 分 ， 并 验证 重 投影 关系 : 


i31 


1 | int main (int argc, char** argv) 


2 | 

3 PE qma 

4 //-- 三 角 化 

5 vector<Point3d> points; 

6 triangulation( keypoints 1, keypoints 2, matches, R, t, points ); 


8 //-- 验证 三 角 化 点 与 特征 点 的 重 投 影 关 系 


9 Mac K = ( Mat <double> ( 3,89 ) «€ E20.9, 0, 325.1, 0, 521.0, 249.7, 0, 0,. 1 2; 
10 for ( int i20; i«matches.size(); i++ ) 
11 1 
12 Point2d pti_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K ); 
13 Point2d pti_cam_3d ( 
14 points[i].x/points[i].z, 
15 points[i].y/points[i].z 
16 ) ; 
17 
18 cout<<"point in the first camera frame: "««ptí cam««endl; 
19 cout<<"point projected from 3D "««pti cam 3d««", d="<<points [i] .z<<end1l; 
20 
21 // 第 2 幅 图 
22 Point2f pt2 cam = pixel2cam( keypoints 2[ matches[i].trainIdx ] .pt, K ); 
23 Mat pt2 trans = R*( Mat «double»(3,1) << points[il.x, points[i].y, points[i 
jm 3 Eus 
24 pt2 trans /= pt2_trans.at<double>(2,0) ; 
25 cout<<"point in the second camera frame: "««pt2 cam««endl; 
26 cout<<"point reprojected from second frame: "««pt2 trans.t()««endl; 
27 cout««endl; 
28 } 
29 if 
30 |] 





我 们 打印 了 每 个 空间 后 在 两 个 相机 华 标 系 下 的 投影 坐标 与 像 系 坐标 
一 一 相当 于 P 的 投影 位 置 与 看 到 的 特征 点 位 置 。 由 于 误 基 的 存在 ， 它 们 
会 有 一 些微 小 的 大 异 。 以 下 古人 条 一 特征 后 的 信息 : 


1 |point in the first camera frame: [0.0844072, -0.0734976] 

2 |point projected from 3D [0.0843702, -0.0743606], d=14.9895 

3 | point in the second camera frame: [0.0431343, -0.0459876] 

4 |point reprojected from second frame: [0.04312769812378599, -0.04515455276163744, 1] 





可 以 看 到 ， 误 大 的 量 级 大 约 在 小 数 点 后 第 3 位 。 可 以 看 到 ， 三 角 化 
特征 点 的 距离 大 约 为 15。 但 由 于 大 度 不 确定 性 ， 我 们 并 不 知 中 这 里 的 15 


究竟 是 多 少 米 。 
7.6.2 ”讨论 


天 于 三 角 训 量 ， 还 有 一 个 必须 注意 的 地 方 。 


三 角 训 量 是 由 平移 ”得 到 的 ， 有 平移 才 会 有 对 极 几 何 中 的 三 角形 ， 
才 谈 得 上 三 角 测 量 。 因 此 ， 纯 旋转 是 无 法 使 用 三 角 测 量 的 ， 因 为 对 极 约 
束 将 永远 满足 。 在 平移 存在 的 情况 下 ， 我 们 还 要 关心 三 角 测 量 的 不 确定 
性 ， 这 会 引出 一 个 三 角 测 量 的 矛盾 。 


如 图 7-10 所 示 ， 当 平移 很 小 时 ， 像 码 上 的 不 确定 性 将 导致 较 大 的 深 
度 不 确定 性 。 也 就 是 说 ， 如 果 特 征 点 运动 一 个 像素 ex ， 使 得 视线 角 变 化 
了 一 个 角度 66 ， 那 么 将 测量 到 深度 值 有 6d 的 变化 。 从 几何 关系 可 以 看 
Fj, Ht KIT, dd 将 明显 变 小 ， 这 说 明 平 移 较 大 时 ， 在 同样 的 相机 分 
汰 率 下 ， 三 角 化 测量 将 更 精确 。 对 该 过 程 的 定量 分 析 可 以 使 用 正弦 定理 
得 到 ， 不 过 这 里 先 考虑 定性 分 析 。 


因此 ， 要 提高 三 角 化 的 精度 ， 其 一 是 提高 特征 点 的 提取 精度 ， 也 束 
是 提 融 图 像 分 辨 紊 一 一 但 这 会 叶 任 图像 变 大 ， 增 加 计算 成 本 。 力 一 廊 式 
是 使 平移 量 增 大 。 但 是 ， 这 会 叶 任 图 像 的 外 观 ” 友 生 明 显 的 变化 ， 比 如 
箱子 原先 被 挡住 的 侧面 显示 出 来 ， 叉 比如 反射 光 肥 生变 化 ， 等 等 。 外 观 
变化 会 使 得 特征 提取 与 匹配 变 得 困难 。 总 而 言 之 ， 再 增 大 平移 ， 会 导致 
匹配 失效 ， 而 平移 太 小 ， 则 三 角 化 精度 不 够 一 一 这 融 是 三 角 化 的 克 导 。 
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图 7-10 三角 测 量 的 矛盾 。 


虽然 本 节 上 只 介绍 了 三 角 化 的 深度 估计， 但 只 要 我 们 愿意 ， 也 能 够 定 
量 地 计算 每 个 特征 点 的 位 置 及 不 确定 性 。 所 以 ， 如 果 假 设 特征 点 服从 
局 其 分布， 并 且 不 断 地 对 它 进行 观测 ， 在 信息 正确 的 情况 下 ， 我 们 束 能 
EREITEA ENA 。 这 束 得 到 了 一 个 滤波 融 ， 称 为 
ARE VEIeas ( Depth Filter) 。 不 过 ， 由 于 所 的 原理 较 复 杂 ， 我 们 将 留 
到 第 13 讲 再 详细 讨论 它 。 下 面 ， 我 们 来 讨论 从 3D-2D 的 匹配 点 来 估计 相 
机 运动 ， 以 及 3D-3D 的 估计 方法 。 


7.7 3D-2D: PnP 


PnP (Perspective-n-Point) 是 求解 3D 到 2D 点 对 运动 的 方法 。 它 摘 述 
了 当知 道 n 个 3D 空 间 点 及 其 投影 位 置 时 ， 如 何 估计 相机 的 位 姿 。 前 面 说 
到 ，2D-2D 的 对 极 几 何方 法 需要 8 个 或 8 个 以 上 的 点 对 (以 八 点 法 为 
Bil) ， 且 存在 着 初始 化 、 纯 旋转 和 尺度 的 问题 。 然 和 而， 如 果 两 张 图 像 中 
其 中 一 张 特 征 点 的 3D 位 置 已 知 ， 那么 最 少 只 需 3 个 点 对 (需要 至 少 一 个 
额外 点 验证 结果 ) 就 可 以 估计 相机 运动 。 特 征 点 的 3D 位 置 可 以 由 三 角 
化 或 者 RGB-D 相 机 的 深度 图 确定 。 因 此 ， 在 双 目 或 RGB-D 的 视觉 里 程 
计 中 ， 我 们 可 以 直接 使 用 PnP 估计 相机 运动 。 而 在 单 目 视 党 里 程 计 中 ， 
必须 先进 行 初始 化 ， 然 后 才能 使 用 PnP。3D-2D 方 法 不 需要 使 用 对 极 约 
束 ， 叉 可 以 在 很 少 的 匹配 点 中 获得 较 好 的 运动 估计 ， 是 最 章 要 有 的 一 种 姿 
人 态 估 计 方 法 。 

PnP 问题 有 很 多 种 求解 方法 ， 例 如 ， 用 3 对 点 估计 位 姿 的 P3Pk5l 、 直 
接线 性 变换 (DLT) 、EPnP (E ffi cientPnP) 4^9 , UPnP^? ， 等 等 。 此 
外 ， 还 能 用 非 线 性 优化 ”的 方式 ， 构 建 最 小 二 乘 问题 并 严 代 求 解 ， 也 束 
是 万 金 油 式 的 Bundle Adjustment。 我们 先 来 看 DLT， 然 后 再 讲解 Bundle 
Adjustment。 


7.7.1 有 下 接线 性 变换 


考虑 某 个 空间 点 P ， 它 的 齐 次 坐标 为 P =(X,Y,Z, 1) 。 在 图 像 I, P, 
投影 到 特征 点 x ;=(u jyv DE (以 归 一 化 平面 齐 次 坐标 表示 )〉 。 此 时 相 1 
的 位 姿 Rt 是 未 知 的 。 与 单 应 和 沧 阵 的 求解 类 似 ， 我 们 定义 增 广 算 阵 [RIt ] 
为 一 个 3x 4 的 和 矩阵， 包含 了 旋转 与 平移 信息 外 。 我 们 将 其 展开 形式 列 写 
如 下 : 


X 
1 ti to t3 t4 
Y 
Siv,|=|ts te t tg (7.26) 
A 
1 to tio tii tie 


IX dd bee Le, EX da +h +h 


Ul = SO OU" FE 一 一 一 一 一 一 . 
toX + tioY + t11Z + 012 i to X + tioY +4117 4+ tie 
= T EN T — T 
ti — (ti, to, t3, t4) , t2 areas (ts, te, t7, tg) , t3 = (to, tio, t11, t12) . 
于 是 有 : 
t P —tiPu, = 0, 
和 
t1 P —t3Pv, = 0. 


IER, t EFRR, APLAR, BME SE SPA Ft 
的 线性 约束 。 假 设 一 共有 MERE, BRI DAS Ee A Be HE TEH: 


Pl 0 uP 

0 Pl -wuPl tà 
. . tə | =0. (7.27) 
PL 0 —unP} | \ts 


0 Pi 一 UN P} 


由 于 t 一 共有 12 维 ， 因 此 ， 最 少 通过 6 对 匹配 点 即 可 实现 窍 阵 了 的 线 


性 求解 ， 这 种 方法 称 为 直接 线性 变换 (Direct Linear Transform, 
DLT)〉。 当 区 配点 大 于 6 对 时 ， 也 可 以 使 用 SVD 等 方法 对 超 定 方程 求 最 


小 二 乘 解 。 

在 DLT 求 解 中 ， 我 们 直接 将 了 矩阵 看 成 了 12 个 未 知 数 ， 忽 略 了 它们 
之 间 的 联系 。 因 为 旋转 矩阵 RE ”SO(3)， 用 DLT 求 出 的 解 不 一 定 满足 该 
约束 ， 它 是 一 个 一 般 和 矩阵 。 平 移 问 量 比 较 好 办 ， 它 属于 问 量 空间 。 对 于 
ese FE BER ， 我 们 必须 针对 DLT 估 计 的 T 左边 3x 3 的 矩阵 块 ， 和 寻找 一 个 
最 好 的 旋转 矩阵 对 它 进 行 近似 。 这 可 以 由 QR 分 解 完成 B41 ， 相 当 于 把 结 
果 从 窍 阵 空间 重新 投影 到 SE(3) 流 形 上 ， 转 换 成 旋转 和 平移 两 部 分 。 

需要 解释 的 是 ， 我 们 这 里 的 x , 使 用 了 归 一 化 平面 坐标 ， 去 挥 了 内 参 
XBEEK 的 影响 这 是 因为 内 参 K 在 SLAM 中 通常 假设 为 已 知 。 即 使 内 
参 未 知 ， 也 能 用 PnP 去 估计 KR,t 三 个 量 。 然 而 由 于 未 知 量 增多 ， 效 果 会 
差 一 些 。 





/./.2 P3P 


下 面 讲 的 P3P 是 另 一 种 解 PnP 的 方法 。 它 仅 使 用 3 对 匹配 点 ， 对 数据 
要 求 较 少 ， 因 此 这 里 也 简单 介绍 一 下 〈 这 部 分 推导 借鉴 了 文献 [49]) 。 

P3P 需 要 利用 给 定 的 3 个 点 的 几何 关系 。 它 的 输入 数据 为 3 对 3D-2D 
匹配 点 。 记 3D 点 为 A,B,C ，2D 点 为 a,b,c ， 其 中 小 写字 母 代 表 的 点 为 对 
应 大 写字 母 代 表 的 点 在 相机 成 像 平 面 上 的 投影 ， 如 图 7-11 所 示 。 此 外 ， 
P3P 还 需要 使 用 一 对 验证 点 ， 以 从 可 能 的 解 中 选 出 正确 的 那 一 个 (类 似 
于 对 极 几 何 情形 ) 。 记 验证 点 对 为 D-d ， 相 机 交心 为 O 。 请 注意 ， 我 们 
知道 的 是 A,B,C 在 世界 坐标 系 中 的 坐标 ， 而 不 是 在 相机 坐标 系 中 的 坐 
ER o 一 旦 3D 点 在 相机 坐标 系 下 的 坐标 能 够 算出 ， 我 们 就 得 到 了 3D-3D 
的 对 应 点 ， 把 PnP 问题 转换 为 了 ICP 问 题 。 


首 乞 ， 显 然 三 角形 之 间 存 在 对 应 天 系 : 


AOab — AOAB, AObc— AOBC, AOac — AOAC. (7.28) 


B 
图 7-11 P3P 问 题 示 意图 。 


来 考虑 Oab MOAB 的 关系 。 利 用 余弦 定理 ， 有 : 
OA" + OB* — 204A - OB - cos (a,b) = AB’. (7.29) 
对 于 其 他 两 个 三 角形 亦 有 类 似 性 质 ， 于 是 有 : 


OA*+OB*—20A.0B.cos(a,b) = AB? 
OB? + OC? — 20B - OC - cos (b, c) = BC? (7.30) 
OA? + OC? — 20A - OC - cos (a,c) = AC*. 


对 以 上 三 式 全 体 除 以 OC*， 并 且 记 x =OA/OC,y -OB/OC , $3: 


t? + y^ — 2zy cos (a,b) = AB^/OC? 
y? + 1? — 2ycos (b, c) = BC?/OC? (7.31) 
z^ + 1^ — 2rcos(a,c) = AC*/OC". 


tv -AB? /OC? ,uv -BC? /OC? ,wv -AC? /OC? , ff: 


z^ + y^ — 2ry cos la, b) — v = 0 
y? + 1? — 2y cos (b,c) — uv = 0 (7.32) 


z^ + 1^ — 2z cos (a, c) — wv = 0. 


我 们 可 以 把 第 一 个 式 子 中 的 v —— BIS aw, FRAR PAX, 


~ 
cju 


1 — wu) y? — ux” — cos (b, c) y + 2uxy cos (a,b) -- 120 
( dy (b, c) y y cos (a, b) (7.33) 


(1 — w) x? — wy? — cos (a, c) x + 2wzy cos (a, b) + 1 = 0. 


注意 这 些 方程 中 的 已 知 量 和 未 知 量 。 由 于 我 们 知道 2D 上 点 的 网 像 位 
置 ，3 个 余 强 角 cos (a,b) , cos (b,c) , cos (ac) ZU AIK. AY, u 
=BC ? /AB ? ,w =AC ? /AB ? 可 以 通过 A,B,C 在 世界 坐标 系 下 的 坐标 算出 ，2 
换 到 相机 坐标 系 下 之 后 ， 这 个 比值 并 不 改变 。 访 式 中 的 xy 是 未 知 的 ， 
随 着 相机 移动 会 发 生变 化 。 因 此 ， 该 方程 组 是 关于 xy 的 一 个 二 元 二 次 
方程 (多项式 方 程 〉》。 解 析 地 求解 该 方程 组 是 一 个 复 森 的 过 程 ， 需 要 用 
肝 消 元 法 。 这 里 不 展开 对 访 方 程 解 法 的 介绍 ， 感 兴趣 的 谈 者 请 参阅 文献 
[45]。 类似 于 分 解 E 的 情况 ， 该 方程 最 多 可 能 得 到 4 个 解 ， 但 我 们 可 以 用 
验证 点 来 计算 最 可 能 的 解 ， 得 到 A,B,C ”在 相机 坐标 系 下 的 3D 坐 标 。 然 
后 ， 根 据 3D-3D 的 点 对 ， 计 算 相 机 的 运动 Rt 。 这 部 分 将 在 7.9 节 介绍 。 


从 P3P 的 原理 可 以 看 出 ， 为 了 求解 PnP， 我 们 利用 了 三 角形 相似 性 
质 ， 求 解 投影 点 op,c 在 相机 坐标 系 下 的 3D 坐 标 ， 最 后 把 问题 转换 成 一 
个 3D 到 3D 的 位 姿 估计 问题 。 在 后 文 将 看 到 ， 珊 有 匹配 信息 的 3D-3D 位 
姿 求 解 非 党 容易 ， 所 以 这 种 思路 是 非常 有 效 的 。 其 他 的 一 些 方法 ， 例 如 
EPnP， 亦 米 用 了 这 种 思路 。 然 而 ，P3P 也 存在 大 一 些 问 题 : 

1.P3P 只 利用 3 个 点 的 信息 。 当 给 定 的 配对 点 多 于 3 组 时 ， 难 以 利用 
更 多 的 信息 。 

2. 如 果 3D 点 或 2D 点 受 噪 声 影响 ， 或 者 存在 误 匹 配 ， 则 算法 失效 。 

所 以 后 续 人 们 还 提出 了 许多 别 的 方法 ， 如 EPnP、UPnP 等 。 它 们 利 
用 更 多 的 信息 ， 而 且 用 迭代 的 方式 对 相机 位 姿 进 行 优 化 ， 以 尽 可 能 地 消 
除 噪 声 的 影响 。 不 过 ， 相 对 于 P3P 来 说 ， 原 理会 更 加 复杂 一 些 ， 所 以 我 
们 建议 读者 阅读 原始 的 论文 ， 或 通过 实践 来 理解 PnP 过 程 。 在 SLAM 当 


中 ， 通 冲 的 做 法 是 先 使 用 P3P/EPnP 等 方法 估计 相机 位 姿 ， 然 后 构建 最 小 
二 溢 优 化 问题 对 估计 值 进行 调整 (Bundle Adjustment) 。 接 下 来 我 们 从 
非 线性 优化 角度 来 看 一 下 PnP 问题 。 


7.7.3 Bundle Adjustment 


除了 使 用 线性 方法 之 外 ， 我 们 还 可 以 把 PnP 问题 构建 成 一 个 定义 于 
李 代 数 上 的 非 线 性 最 小 二 乘 问 题 。 这 将 用 到 本 书 第 4 讲 和 第 5 讲 的 知识 。 
前 面 说 的 线性 方法 ， 往 往 是 完 求 相机 位 姿 ， 再 求 空间 点 位 置 ”， 而 非 线 
性 优化 则 是 把 它们 都 看 成 优化 变量 ， 放 在 一 起 优化 。 这 是 一 种 非常 通用 
的 求解 方式 ， 我 们 可 以 用 它 对 PnP 或 ICP 给 出 的 结果 进行 优化 。 在 PnP 
中 ， 这 个 Bundle Adjustment 问 题 ， 是 一 个 最 小 化 重 投影 误 莽 C 
Reprojection error) ”的 问题 。 我 们 将 在 本 节 给 出 此 问题 在 两 个 视图 下 的 
基本 形式 ， 然 后 在 第 10 讲 讨论 较 大 规模 的 BA 问题 。 


考虑 mn 个 三 维 空间 点 P 及 其 投影 p ， 我 们 和 希望 计算 相机 的 位 姿 Rit ， 
它 的 李 代 数 表 示 为 E 。 假 设 菜 空 间 点 坐标 为 P, -[X,, Y, ,Z, E, RRS 
Zhi Au, =[u, v, J 。 根 据 第 5 讲 的 内 容 ， 像 系 位 置 与 空间 点 位 置 的 关 
系 如 下 : 


Xi 
Ui 
Yi 
si | v; | = Kexp(£^) (7.34) 
Zi 
1 
1 


ER HE ANAM AS Zh, AU AA TEE EC RE Co 3 X 
FE PEJE Sa oe : 


siu; = K exp (£^) Bj. 


请 读者 脑 补 中 间 隐 含 着 的 齐 次 坐标 到 非 齐 次 的 转换 ， 和 否则 按 矩 阵 的 
乘法 来 说 ， 维 度 是 不 对 的 回 。 。 现 在 ， 由 于 相机 位 姿 未 知 及 观测 点 的 品 


声 ， 该 等 式 和 存在 一 个 误 牵 。 因 此 ， 我 们 把 误 开 求 和 ， 构 建 最 小 二 乘 问 
题 ， 然 后 寻找 最 好 的 相机 位 姿 ， 使 它 了 最 小 化 : 


2 


1 
u; — —K exp(£^) Pj (7.35) 
Si D 














t =argmin5 2, 


该 问题 的 误差 项 ， 是 将 像素 坐标 (观测 到 的 投影 位 置 ) 与 3D 点 按 
照 当 前 估计 的 位 姿 进 行 投 影 得 到 的 位 置 相 比较 得 到 的 误差 ， 所 以 称 为 重 
投影 误差 。 使 用 齐 次 坐标 时 ， 这 个 误差 有 3 维 。 不 过 ， 由 于 u 最 后 一 维 
为 1， 该 维度 的 误 兰 一 直 为 零 ， 因 而 我 们 更 多 时 候 使 用 非 齐 次 坐标 ， 于 
是 误 关 就 只 有 2 维 了 。 如 图 7-12 所 示 ， 我 们 通过 特征 匹配 知道 了 Jp ， 和 P , 
是 同一 个 空间 点 P 的 投影 ， 但 是 不 知道 相机 的 位 姿 。 在 初始 值 中 ，P 的 


得 这 个 距离 变 小 。 不 过 ， 由 于 这 个 调整 需要 考虑 很 多 个 点 ， 所 以 最 后 每 
个 点 的 误 兰 通 间 都 不 会 精确 为 零 。 

最 小 二 乘 优化 问题 已 经 在 第 6 讲 介 绍 过 了 。 使 用 李 代 数 ， 可 以 构建 
无 约束 的 优化 问题 ， 很 方便 地 通过 高 斯 牛顿 法 、 列 文 介 格 一 乌 伪 尔 特 方 
法 等 优化 算法 进行 求解 。 不 过 ， 在 使 用 高 斯 牛顿 法 和 列 文 介 格 一 马 仿 尔 
特 方法 之 前 ， 我 们 需要 知道 每 个 误 天 项 基于 优化 变量 的 寻 数 ， 也 吏 是 线 
性 化 : 


e(x + Ax) = e(a) + JAa. (7.36) 


图 7-12 重 投 影 误 基 示 意图 。 


这 里 的 J 的 形式 是 值得 讨论 的 ， 甚 至 可 以 说 是 关键 所 在 。 我 们 固然 
可 以 使 用 数值 导数 ， 但 如 果 能 够 推导 出 解析 形式 ， 则 我 们 会 优先 考虑 解 
析 导 数 。 现 在 ， 当 e “为 像素 坐标 误差 QW) ，x “为 相机 位 姿 《〈6 维 ) 
时 ， 了 将 是 一 个 2x 6 的 矩阵 。 我 们 来 推导 本 的 形式 。 

回忆 有 他 代数 的 内 容 ， 我 们 介绍 了 如 何 使 用 扰动 模型 来 求 李 代数 的 导 
数 。 首 先 ， 记 变换 到 相机 坐标 系 下 的 空间 点 坐标 为 P ， 并 有 日 将 其 前 3 维 
取出 来 : 


P (ept Py. = VY T. (7.37) 


那么 ， 相 机 投影 模型 相对 于 P 为 


su = KP’. (7.38) 
展开 : 
su Is 4 Gy A 
w lel 8 X. G Y pn. (7.39) 
S 0 0 1 g 


利用 第 3 行 消 去 s CSEbs ESUEP 的 距离 ) , fF: 


/ 


x ba 
i= jz Fep U= Ii T En (7.40) 


这 与 之 前 讲 的 相机 模型 是 一 致 的 。 当 我 们 求 误差 时 ， 可 以 把 这 里 的 
uv SSE NWS (BLE, KE. EMS Peele, RE AR 
扰动 量 65 ， 然 后 考虑 e 的 变化 天 于 扰动 量 的 导数 。 利 用 链 式 法 则 ， 可 以 
Aul: 


Oe — , e(6€ BE) Ge OP" (7.41) 


0ó£  s&£2o0 ôE OP’ asé 
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Ne 


x He FBeALL EN A. BOER d DG EA: 
在 陈 (7.40) 已 经 列 出 了 变量 之 间 的 关系 ， 易 得 : 








Ou Ou Ou fa uf A 
Oe ax’ OY’ az! = Z! 0 VAR T 
po — a ' (7.42) 
OP Ov Ov Ov 0 fy fuY 
Ox’ oy! OZ! Z! Z2 


UTD) = (TP)? = | _ | l (7.43) 


MEP 的 定义 中 ， 我 们 取出 了 前 3 维 ， 于 是 得 : 


OP! — 
DoE | 


将 这 两 项 相 习 ， 束 得 到 了 2x 6 的 雅 可 比 矩 阵 : 





1,—P’|. (7.44) 











z 2X! zXX zX? zY' 
Oe fe 0 is ad Z2 MT m Ee — it (7 45) 
JE à & ARE 4-9"  nLEBO Ax | 
Zi Z2 y Z2 Z2 FA 
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关系 。 我 们 你 留 了 前 面 的 负 号 ， 这 是 因为 误 达 十 由 观测 值 减 预测 值 ” 定 
义 有 的 。 它 当然 也 可 反 过 来 ， 定 义 成 “预测 值 减 观 误 值 ”的 形式 。 在 那 种 情 
况 下 ， 只 要 去 挥 前 面 的 负 写 即 可 。 此 外 ， 如 果 se(3) 的 定义 方式 是 旋转 在 
前 ， 平 移 在 后 ， 只 要 把 这 个 矩阵 的 前 3 列 与 后 3 列 对 调 即 可 。 


尺 一 方面 ， 除 了 优化 位 姿 ， 我 们 还 希望 优化 特征 点 的 空间 位 置 。 


此 ， 和 需要 讨论 e 关于 空间 点 P 的 导数 。 所 六 这 个 吕 数 矩阵 相对 来 说 容易 
一 些 。 仍 利用 链 式 法 则 ， 有 : 


Oe De OP’ 





ae ET (7.46) 
第 一 项 在 前 面 己 推 寻 ， 天 于 第 二 项 ， 按 照 定义 
P' — exp(£^)P — RP +t, 
我 们 友 现 P' 对 P ORS AR RR Se: 
[a ft! 
a —- | 7 M Ee | R. (7.47) 





于 和 是， 我们 推导 出 了 观测 相机 方程 关于 相机 位 姿 与 特征 点 的 两 个 寻 
DUE. EN tari, BÉUEXEDUMONRE Spek BIN ER Al, 18 
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7.8 SE: 求解 PnP 


7.81 ”使 用 EPnP 求 解 位 姿 


下 面 ， 我 们 通过 实验 理解 一 下 PnP 的 过 程 。 首 先 ， 我 们 用 OpenCV 提 
供 的 EPnP 求 和 解 PnP 问 题 ， 然 后 通过 g2o 对 结果 进行 优化 。 由 于 PnP 需要 使 
用 3D 上 点， 为 了 避免 初始 化 市 来 的 国 烦 ， 我 们 使 用 了 RGB-D 相 机 中 的 深 
FER (1 depth.png) 作为 特征 点 的 3D 位 置 。 背 先 来 看 OpenCV 提 供 的 
PnP PŽ: 


四 slambook/ch7/pose estimation 3d2d.cpp C Fr EX) 


1 | int main( int argc, char** argv ) 


4 zl 建立 3D 点 


5 Mat dl = imread( argv[3], CV LOAD IMAGE UNCHANGED ); // 深度 图 为 16 位 无 
符号 数 ， 单 通道 图 像 

6 Mat K = ( Mat, «double» ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 

7 vector<Point3f> pts 3d; 

8 vector<Point2f> pts. 2d; 

9 for ( DMatch m:matches ) 

10 1 


11 ushort d = d1.ptr<unsigned short? ( int(keypoints 1[m.queryIdx].pt.y) )[ 
int (keypoints_1[m.queryIdx].pt.x) ]; 


12 if (d==0) // bad depth 

13 continue; 

14 float dd = d/1000.0; 

15 Point2d pi = pixel2cam( keypoints 1[m.queryIdx].pt, K ); 
16 pts_3d.push_back( Point3f(pi.x*dd, pi.y*dd, dd) ); 

17 pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); 

18 } 


20 cout««"3d-2d pairs: "««pts, 3d.size()««endl; 


Nat r, di 

// 调用 OpenCV 的 PnP 求解 ， 可 选择 EPNP, DLS 等 方法 

solvePnP( pts 3d, pts 2d, K, Mat(), r, t, false, cv::SOLVEPNP EPNP ); 
Mat R; 

cv::Rodrigues(r, R); // r 为 旋转 向 量 形 式 ， 用 Rodrigues 公式 转换 为 矩阵 


cout««"Rz"««endl««R««endl; 


cout««"tz"««endl««t««endl; 





在 例 程 中 ， 得 到 配对 特征 点 后 ， 我 们 在 第 一 个 图 的 深度 图 中 寻找 它 


们 的 深度 ， 并 求 出 空间 位 置 。 以 此 空间 位 置 为 3D 点 ， 再 以 第 二 个 图 像 
的 像素 位 置 为 2D 点 ， 调 用 EPnP 求 解 PnP 问 题 。 程 序 输出 如 下 : 


A build/pose estimation 3d2d 1.png 2.png di.png d2.png 


-- Max dist : 95.000000 
-- Min dist : 4.000000 
一 共 找 到 了 79 组 匹配 点 
3d-2d pairs: 78 


[0.9977970937403702, -0.05195299069131867, 0.04125344205637558; 
0.05073872610592159, 0.9982626103770279, 0.02995567385972873; 
-0.04273805559942161, -0.02779653722084675, 0.9986995599889442] 
t- 

[-0.6455324432075111; 

-0.05776758294184359; 

0.2844565219506077] 





读 痢 可 以 对 比 先前 2D-2D 人 情况 下 求解 的 Rt 看 看 有 什么 个 同 。 可 以 看 


到 ， 在 有 3D 信 息 时 ， 估 计 的 R 几乎 是 相同 的 ， 而 t FAAS. EH 
于 引入 了 新 的 深度 信息 所 致 。 不 过 ， 由 于 Kinect 采 集 的 深度 图 本 号 会 有 
一 些 误 差 ， 所 以 这 里 的 3D 点 也 不 是 准确 的 。 我 们 会 希望 把 位 姿 E 和 所 有 
三 维特 征 点 P 同时 优化 。 


7.8.2 ”使 用 BA 优化 


下 面 汗 示 如 何 进行 BundleAdjustment。 我 们 将 使 用 前 一 步 的 估计 值 
作为 初始 伸 。 优 化 可 以 使 用 前 面 讲 的 Ceres 或 g20 库 实现 ， 这 里 采用 g20 
作为 例子 。 

g20 的 基本 知识 在 第 6 讲 中 己 经 介绍 过 了 。 在 使 用 g20 之 前 ， 我 们 要 
把 问题 建 模 成 一 个 最 小 二 乘 的 图 优化 问题 ， 如 图 7-13 所 示 。 






P, g20::VertexSBAPointXYZ 


g20::EdgeProjectXYZ2UV 


z, - h(& E) 


XE em À exp(£" ) 
R,t 
g20::VertexSE3Expmap 


图 7-13 ”PnP 的 Bundle Adjustment 的 图 优化 表示 。 


在 这 个 图 优化 中 ， 节 点 和 边 的 选择 如 下 : 

LER : 第 二 个 相机 的 位 姿 节 点 EE se(3)， 所 有 特征 点 的 空间 位 置 
PER. 

2. 边 : BESBSD ATE T ABL Ace, AIM AFERA: 

a hie, £5). 

由 于 第 一 个 相机 位 姿 回 定 为 零 ， 我 们 没有 把 它 写 到 优化 变量 里 ， 但 
在 习题 中 ， 硕 望 你 能 够 把 第 一 个 相机 的 位 姿 与 观测 也 考虑 进来 。 现 在 我 
们 根据 一 组 3D 点 和 第 二 个 图 像 中 的 2D 投 影 ， 佑 计 第 二 个 相机 的 位 姿 。 
所 以 我 们 把 第 一 个 相机 男 成 虚线 ， 表 明 不 希望 考虑 它 。 

g20 提 供 了 许多 关于 BA 的 节点 和 边 ， 我 们 不 必 自 己 从 头 实 现 所 有 的 


计算 。 在 g2o/types/sba/types_six_dof expmap.h 中 则 提供 了 李 代 数 表 达 的 
节点 和 边 。 请 读者 打开 这 个 文件 ， 找 到 VertexSE3Expmap〈 全 代数 位 
姿 ) . VertexSBAPointXYZ (FE AMEA) 和 EdgeProjectXYZ2UV (〈 投 
影 方程 边 ) 这 三 个 类 。 我 们 来 简单 看 一 下 它们 的 类 定义 ， 例 如 
VertexSE3Expmap: 





class G20_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{ 


public: 
EIGEN MAKE ALIGNED OPERATOR NEW 


VertexSE3Expmap () ; 


bool read(std::istream& is); 


o oo “I Cn un = o to 一 


bool write(std::ostream& os) const; 


1 virtual void setToOriginImpl() { 


12 _estimate = SE3Quat(); 

13 } 

14 

15 virtual void oplusImpl(const double* update ) { 
16 Eigen: :Map<const Vector6d> update(update ); 

17 setEstimate( SE3Quat::exp(update)*estimate()); 
18 } 

i9 |}; 





请 注意 它 的 模板 参数 。 第 一 个 参数 6 表示 它 内 部 存储 的 优化 变量 维 
度 ， 可 以 看 到 这 是 一 个 6 维 的 李 代 数 。 第 二 个 参数 是 优化 变量 的 类 型 ， 
这 里 使 用 了 g2o 定 义 的 相机 位 姿 : SE3Quat。 这 个 类 内 部 使 用 了 四 元 数 
加 位 移 同 量 来 存储 位 姿 ， 但 同时 也 文 持 孚 代数 上 的 运算 ， 例 如 对 数 映 射 
(log 函 数 ) 和 李 代 数 上 增 量 (update 函数 ) 等 操作 。 我 们 可 以 对 照 它 的 
实现 代码 ， 看 看 g2o 对 李 代 数 是 如 何 操作 的 : 


1 | class G20 TYPES SBA API VertexSBAPointXYZ : public BaseVertex<3, Vector3D> 


6 |class G20 TYPES SBA API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, 
VertexSBAPointXYZ, VertexSE3Expmap> 


9 void computeError() { 

10 const VertexSE3Expmap* vi = static cast«const VertexSE3Expmap*»^( vertices 
[1]) ; 

1 const VertexSBAPointXYZ* v2 = static cast«const VertexSBAPointXYZ*> ( 


_vertices[0]); 


12 const CameraParameters * cam 
13 = static_cast<const CameraParameters *>(parameter(0)) ; 
14 Vector2D obs(_measurement) ; 


15 _error = obs-cam-»cam map(vi-»estimate().map(v2-»estimate())); 
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位 置 类 的 维度 为 3， 类 型 是 Eigen 的 Vector3D。 方面 ， 边 
EdgeProjectXYZ2UV 连 接 了 前 面 说 的 两 个 顶点， 它 的 观测 值 为 2 维 ， 由 
Vector2D 和 表示 ， M RE E PIR EITE RIA 
了 投影 方程 的 误差 计算 方法 ， 也 残 是 我 们 前 面 提 到 的 zh (6&,P ) 的 方式 。 


现在 ， 进 一 步 观 察 EdgeProjectXYZ2UV 的 linearizeOplus 函 数 的 实 
现 。 这 里 用 到 了 我 们 前 面 推导 的 雅 可 比 定 阵 : 


void EdgeProjectXYZ2UV: :linearizeOplus() { 
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]); 
SE3Quat T(vj->estimate()); 
VertexSBAPointXYZ* vi = static cast«VertexSBAPointXYZ*»( vertices[0]); 
Vector3D xyz = vi-»estimate(); 


Vector3D xyz trans = T.map(xyz); 


double x = xyz trans[0]; 
double y = xyz trans[1]; 
double z = xyz trans[2]; 
double z 2 = z*z; 


const CameraParameters * cam = static cast«const CameraParameters *>(parameter 


(0) ) ; 


Matrix<double,2,3,Eigen::ColMajor> tmp; 
tmp(0,0) = cam->focal_length; 

tmp(0,1) = 0; 

tmp(0,2) = -x/z*cam-»5focal length; 


tmp(1,0) = 0; 
tmp(1,1) = cam-»focal length; 
tmp(1,2) = -y/z*cam-»^focal length; 


_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix(); 


.jacobianÜ0plusXj(0,0) = x*y/z 2 *cam-»focal length; 
.jacobianO0plusXj(0,1) = -(1+(x*x/z_2)) *cam-»focal, length; 
_jacobianOplusXj(0,2) = y/z *cam-»focal length; 
_jacobianOplusXj (0,3) = -1./z *cam-»focal length; 
_jacobian0Oplusxj(0,4) = 0; 

_jacobian0plusxj(0,5) = x/z_2 *cam-»focal, length; 


_jacobianOplusxXj(1,0) = (1*yx*y/z 2) *cam-»focal length; 
.jacobianO0plusXj(1,1) = -x*y/z 2 *cam-»focal length; 
.jacobianO0plusXj(1,2) = -x/z *cam-»focal, length; 
.jacobianOplusXj(1,3) = 0; 

_jacobian0Oplusxj(1,4) = -1./z *cam-»^focal, length; 
.jacobianOplusXj(1,5) = y/z 2 *cam-»focal length; 


仔细 研究 此 上 段 代码 ， 我 们 会 发 现 它 与 式 (7.45) 和 (7.47) 是 一 至 的。 成 
员 变 量 “_jacobianOplusXi” 是 误 鼻 到 空间 点 的 导 
数 ，“_jacobianOplusXj” 是 误 关 到 相机 位 姿 的 导数 ， 以 至 代数 的 左 乘 扰动 
#218. FAA ZAIN Ee, g2oMK ALAS 统一 拍 述 f. ,f, ， 并 且 李 代数 定义 
顺序 不 同 〈g2o 是 旋转 在 前 ， 平 移 在 后 ; 我们 是 平移 在 前 ， 旋 转 在 
Ja) ， 上 所 以 宅 阵 前 3 列 和 后 3 列 与 我 们 的 定义 是 相反 的 ， 此 外 都 一 致 。 
值得 一 提 的 是 ， 我 们 亦 可 日 己 实现 相机 位 姿 节 点 ， 并 使 用 
Sophus::SE3 来 表达 位 姿 ， 提 供 类 似 的 求 导 过 程 。 然 而 ， 既 然 g20 已 经 所 
供 了 这 样 的 类 ， 在 没有 额外 要 求 的 情况 下 ， 目 己 睾 新 实现 就 没有 必要 
了 。 现 在 ， 我 们 在 上 一 个 PnP 例 程 的 基础 上 ， 加 上 g2o 提 供 的 Bundle 
Adjustment。 


四 slambook/ch7/pose estimation 3d2d.cpp (Fr EX) 


l 


void bundleAdjustment ( 


const vector< Point3f > points 3d, 
const vector< Point2f > points 2d, 
const Mat& K, 

Mat& R, Mat& t ) 


// 初始 化 g20 


typedef g20::BlockSolver< g20::BlockSolverTraits<6,3> > Block; // pose 维度 为 


6, landmark 维度 为 3 


Block::LinearSolverType* linearSolver = new g20::LinearSolverCSparse<Block:: 


PoseMatrixType>() ; 

Block* solver_ptr = new Block( linearSolver ); 
g20::ÜptimizationAlgorithmLevenberg* solver = new g20:: 
OptimizationAlgorithmLevenberg( solver_ptr ); 
g20::SparseÜptimizer optimizer; 


optimizer.setAlgorithm( solver ); 


// vertex 
g20::VertexSE3Expmap* pose = new g20::VertexSE3Expmap(); // camera pose 
Eigen: :Matrix3d R mat; 
R_mat << 
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), 
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), 
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2); 
pose->setId(0) ; 
pose->setEstimate( g2o0: :SE3Quat ( 
R_mat, 


Eigen: :Vector3d( t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0)) 


) 23 


optimizer.addVertex( pose ); 


int index = 1; 


for ( const Point3f p:points 3d ) // landmarks 


1 
g20::VertexSBAPointXYZ* point = new g20o::VertexSBAPointXYZ(); 
point->setId( index** ); 
point->setEstimate( Eigen::Vector3d(p.x, p.y, p.z) ); 
point->setMarginalized( true ); 
optimizer.addVertex( point ); 

} 


// parameter: camera intrinsics 

g20::CameraParameters* camera = new g20::CameraParameters( 
K.at<double>(0,0), Eigen: :Vector2d(K.at<double>(0,2), K.at<double>(1,2)), 0 
); 

camera-»setId(0); 


optimizer.addParameter( camera ); 


// edges 

index = 1; 

for ( const Point2f p:points 2d ) 

1 
g20::EdgeProjectXYZ2UV* edge = new g20o::EdgeProjectXYZ2UV(); 
edge-»setId( index ); 
edge->setVertex( 0, dynamic, cast«g20::VertexSBAPointXYZ*» (optimizer.vertex 
(index)) ); 
edge->setVertex( 1, pose ); 
edge->setMeasurement( Eigen::Vector2d( p.x, p.y ) ); 
edge-»setParameterId(0,0); 
edge->setInformation( Eigen::Matrix2d::Identity() ); 
optimizer .addEdge (edge) ; 


index+t+; 


chrono::steady clock::time point tí chrono::steady clock::now(); 
optimizer.setVerbose( true ); 

optimizer.initializeOptimization() ; 

optimizer .optimize (100) ; 

chrono::steady clock::time point t2 = chrono::steady_clock: :now() ; 

chrono: :duration<double> time used = chrono::duration_cast<chrono: :duration< 
double>>(t2-t1) ; 


cout<<"optimization costs time: "<<time_used.count()<<" seconds."««endl; 


cout<<endl<<"after optimization:"««endl; 


cout««"T2"««endl««Eigen::Isometry3d( pose->estimate() ).matrix()««endl; 


程序 大 体 上 和 第 6 讲 的 g20 类 似 。 我 们 自 完 声明 了 g20 图 优化 ， 并 配 
曾 优 化 求解 融和 梯度 下 降 方 法 。 然 后 根据 估计 a 到 的 特征 点 ， 将 位 姿 和 空 
同 点 放 到 图 中 。 最 后 调用 优化 函数 进行 求解 。 读 痢 可 以 看 到 优化 的 结 
如 下 : 


1 | calling bundle adjustment 

2 |iteration- 0 chi2- 1.083180 time- 0.000107183 cumTime- 0.000107183 edges- 76 schur 
- ] lambda- 78.907222 levenbergIter- 1 

3 |iteration- 1 chi2= 0.000798 time= 5.8615e-05 cumTime= 0.000165798 edges- 76 schur 
- 1 lambda- 26.302407 levenbergIter- 1 

4 |iteration- 2 chi2= 0.000000 time- 3.0203e-05 cumTime= 0.000196001 edges= 76 schur 
- 1 lambda- 17.534938 levenbergIter- 1 

5 中 间 过 程 略 

6 |iteration- 11 chi2= 0.000000 time- 2.8394e-05  cumTime- 0.000525203 edges= 76 schur 


- 1 lambda- 11209.703029 levenbergIter- 1 


7 | optimization costs time: 0.00132938 seconds. 


9 | after optimization: 

10 | T= 

1 | 0.997776 -0.0519476 0.0417755 -0.649778 

12 |0.050735 0.998274 0.0295806 -0.0545231 

3 | -0.0432401 -0.0273953 0.998689 0.295564 
4 | 0 0 0 1 
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变 ， 于 是 俘 止 优化 。 我 们 输出 了 最 后 得 到 的 位 姿 变 换 矩 阵 了 ， 对 比 之 前 
和 耳 接 做 PnP 的 结果 ， 大 约 在 小 数 扣 后 第 3 位 友 生 了 一 些 变 化 。 这 主要 是 由 
于 我 们 同时 优化 了 特征 点 和 相机 位 姿 导 致 的 。 


Bundle Adjustment 是 一 种 通用 的 做 法 。 它 可 以 不 限于 两 幅 图 像 。 我 
们 完全 可 以 放 入 多 幅 图 像 匹 配 到 的 位 姿 和 空间 点 进行 达 代 优化 ， 其 至 可 
以 把 整个 SLAM 过 程 放 进来 。 那 种 做 法 规模 较 大 ， 主 要 在 后 问 使 用 ， 我 
们 会 在 第 10 讲 再 次 遇 到 这 个 问题 。 在 前 病 ， 我 们 通 间 考虑 局 部 相机 位 姿 
和 特征 点 的 小 型 Bundle Adjustment} el, A EI E RET SEI RAIL 
Mo 


7.9 3D-3D: ICP 


最 后 ， 我 们 来 介绍 3D-3D 的 位 姿 估 计 问 题 。 假 设 我 们 有 一 组 配对 好 
的 3D 点 (比如 我 们 对 两 幅 RGB-D 图 像 进行 了 匹配 ): 


P = {pi1,.… ,pn}, P’ = fpi wn, 
现在 ， 想 要 找 一 个 欧 氏 变换 R,t ， 使 得 : 
Vi, p; = Rp; +t. 


这 个 问题 可 以 用 迭代 最 近 点 〈Iterative Closest Point, ICP) 求解 。 
读者 应 该 注意 到 了 ，3D-3D 位 姿 估 计 问 题 中 并 没有 出 现 相 机 檬 型， 也 就 
是 说 ， 仪 考虑 两 组 3D 点 之 则 的 变换 时 ， 和 相机 并 没有 关系 。 因 此 ， 在 
激光 SLAM 中 也 会 碰 到 ICP， 不 过 由 于 激光 数据 特征 不 够 丰富 ， 我 们 无 
从 知道 两 个 点 集 之 间 的 匹配 关系 ， 只 能 认为 距离 最 近 的 两 个 点 为 同一 
个 ， 所 以 这 个 方法 称 为 迭代 最 近 点 。 而 在 视觉 中 ， 特 征 点 为 我 们 提供 了 
较 好 的 匹配 关系 ， 所 以 整个 问题 瓯 变 得 更 简单 了 。 在 RGB-D SLAM 中 ， 
可 以 用 这 种 方式 估计 相机 位 姿 。 下 文 我 们 用 ICP 指 代 匹 配 好 的 两 组 点 间 
的 运动 估计 问题 。 

和 和 PnP 类似，ICP 的 求解 也 分 为 两 种 方式 利用 线性 代数 的 求解 〈 主 
要 是 SVD) ， 以 及 利用 非 线 性 优化 方式 的 求解 “类 似 于 Bundle 
Adjustment) 。 下 面 分 别 进行 介绍 。 


7.9.4 SVD 方 法 


首先 来 看 以 SVD 为 代表 的 代数 方法 。 根 据 前 面 描述 的 ICP 问 题 ， 我 
们 先 定义 第 i 对 点 的 误差 项 : 


e; = pi — (Rp; + t). (7.48) 


MI, Pale) epe, ORE AEP AIK BBS HR, : 


min J = 5 ; D lo: — (Rp;! + t))||2. (7.49) 


下 面 来 推导 它 的 求解 方法 。 首 先 ， 定 义 两 组 点 的 质心 : 


p= 二》 0p. p' = = 5p). (7.50) 


itm, WDA PR. ba, ERA RA OP bee 


1 Y lipi- (Rp; +t)? =4 Y lipi- Rp/ —t— p+ Rp! +p- Rp’ |’ 
=} È Ili -p - R(p/ —p’)) + (p - Rp' — tl 
= 1*lp; —p — R(pi’ — p') | + |lp — Rp’ 一 地 + 
2(Pi - p - R(p/ — p) (p- Rp’ - t)). 
注意 到 交 文 项 部 分 中 (p; -P-R (p, -P )f2X HLZ ENE, HERE H 
标 函 数 可 以 简化 为 


2 
min J = 5 LS p. - p- R(p/ -p| + lp - Rp’ 一 让. (7.51) 


q=] 


仔细 观察 左右 两 项 ， 我 们 发 现 左边 只 和 旋转 矩阵 R 相关 ， 而 右边 既 
HR 也 有 t ， 但 只 和 质心 相关 。 入 要 找 们 获得 了 R > SR DUCERE 
得 到 t 。 于 是 ，ICP 可 以 分 为 以 下 三 个 步骤 求解 : 


1. 计 算 两 组 点 的 质心 位 置 p,p ， 然 后 计算 每 个 点 的 去 质心 坐标 : 
qi 一 Pi 一 D， di 一 Di 一 2 


2. 根 据 以 下 优化 问题 计算 放 转 矩阵 : 


1 TL m 
P= in 一 ;— Rg; 7,52 
argmin 3 2 lla q;| (7.52) 


3.48 TG £8 22V AYR 计算 t: 
t =p- Rp’. (7.53) 

我 们 看 到 ， 只 要 求 出 了 两 组 点 之 间 的 旋转 ， 平 移 量 是 非常 容易 得 到 
的 。 上 所 以 我 们 重点 关注 民 的 计算 。 展 开关 于 R 的 误差 项 ， 得 : 

ix i 

E i lg; — Rg!" = - 2. qi qi +g; R Rq; — 2q} Rq;. (7.54) 

注意 到 第 一 项 和 R Zu OULASTRIR=I, JRERJZGX. BD, 
实际 上 优化 目标 函数 变 为 


> -q Rq; = >》 -tr(Rqiq;) = —tr (> 23 | (7.55) 
2 一 ] | 


ped 


接 下 来 ， 我 们 介绍 怎样 通过 SVD 解 出 上 述 问题 中 最 优 的 R o KTE 
优 性 的 证 明 较 为 复 人 杂 ， 感 兴趣 的 读者 请 参考 文献 [50,51]。 为 了 解 R . 4c 
定义 矩阵 : 


W = >》 qig' - (7.56) 
i=l 


W 是 一 个 3x ZISERE, XIW 3utfTSVDAff£, $8: 
W = UV.. (7.57) 


AU. DAT SHEA RIN SAREE, PAAR TCA MA BID AES, 
而 U 和 V AX FARES. SW WAN, RA 


R=UV'. (7.58) 


解 得 R 后 ， 按 式 (7.53) 求 解 t 即 可 。 


7.9.2” 非 线性 优化 方法 


求解 ICP 的 为 一 种 方式 古 使 用 非 线性 优化 ， 以 达 代 的 方式 去 找 最 优 
值 。 访 方法 和 我 们 前 面 讲 述 的 PnP 非 第 相似 。 以 至 代数 表达 位 姿 时 ， 目 
标 图 数 可 以 与 成 


1 TU 
min = 5 / (pi — exp (£^) pl (7.59) 
i=] 


FS RAIMA TMZ SPE TC St EA TR BY 
Hf: 


de _ 
00€ — 


To. ÆFIR EREE R EAR, bBedesydMB. mH. n 
DAWEH!) ” ，ICP 问 题 存在 唯一 解 或 无 穷 多 解 的 情况 。 在 唯一 解 的 情况 
下 ， 只 要 能 找到 极 小 值 解 ， 那 么 这 个 极 小 值 就 是 全 局 最 优 值 一 一 因此 
不 会 过 到 局 部 极 小 而 非 全 局 最 小 的 情况 。 这 也 意味 看 ICP 求 解 可 以 任意 
选 定 初 始 值 。 这 是 已 匹配 点 时 求解 ICP 的 一 大 好 处 。 


需要 说 明 的 是 ， 我 们 这 里 讲 的 ICP 是 指 已 由 图 像 特征 给 定 了 [匹配 的 
情况 下 进行 位 姿 估计 的 问题 。 在 匹配 已 知 的 情况 下 ， 这 个 最 小 二 乘 问题 
实际 上 具有 解析 解 5253559 ， 所 以 并 没有 必要 进行 迭代 优化 。ICP 的 研究 
者 们 往往 更 加 关心 匹配 未 知 的 情况 。 不 过 ， 在 RGB-D SLAM 中 ， 由 于 一 
个 像 系 的 深 破 数据 可 能 测量 不 到 ， 所 以 我 们 可 以 混合 看 使 用 PnP 和 ICP 优 
化 : 对 于 深度 已 知 的 特征 点 ， 建 模 它 们 的 3D-3D 误 兰 ; 对 于 深度 未 知 的 
特征 点 ， 则 建 模 3D-2D 的 重 投影 误差 。 于 是 ， 可 以 将 所 有 的 误差 放 在 同 
一 个 问题 中 考虑 ， 使 得 求解 更 加 方便 。 


—(exp (£^) pi). (7.60) 


7.10 SEER: 求解 ICP 


7.10.1 ”SVD 方法 


下 面 演示 一 下 如 何 使 用 SVD 及 非 线 性 优化 来 求解 ICP。 本 节 我 们 使 
用 两 幅 RGB-D 图 像 ， 通 过 特征 匹配 获取 两 组 3D 点 ， 最 后 用 ICP 计 算 它 们 
的 位 姿 变 换 。 由 于 OpenCV 目 前 还 没有 计算 两 组 币 岂 配点 的 ICP 的 方法 ， 
而 且 它 的 原理 也 并 不 复杂 ， 所 以 我 们 目 己 来 实现 一 个 ICP。 


四 slambook/ch7/pose estimation 3d3d.cpp C Fr Ex) 


void pose estimation 3d3d( 
const vector<Point3f>& pts1, 
const vector<Point3f>& pts2, 
Mat& R, Mat& t 


Point3f pi, p2; // center of mass 
int N = ptsi.size(); 
for ( int i20; i<N; i++ ) 
1 
pi += ptsi[i]; 
p2 += pts2[il; 
} 
pi /= N; p2 /= N; 
vector<Point3f> q1(N), q2(N); // remove the center 
for ( int i=0; i<N; i++ ) 


{ 


qi [il 
q2 [il] 


ptsi [il = pij 
pts2[i] = p2; 


// compute g1*q2T 

Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); 

for ( int i20; i<N; i++ ) 

{ 
W += Eigen: :Vector3d( gili].x, qilil.y, gili].z ) * Eigen::Vector3d( q2[i]. 
x, q2lil.y, q2[i].z ).transpose() ; 

} 


cout<<"W="<<W<<end1; 


// SVD on W 
Eigen: : JacobiSVD<Eigen: :Matrix3d> svd(W, Eigen: :ComputeFullU|Eigen: : 
ComputeFullV); 


32 Eigen::Matrix3d U = svd.matrixU(); 


33 Eigen::Matrix3d V = svd.matrixV(); 

34 cout««"Uz"««U««endl; 

35 cout<<"V="<<V<<end1; 

36 

37 Eigen: :Matrix3d R_ = U*(V.transpose()); 

38 Eigen: :Vector3d t_ = Eigen::Vector3d( pi.x, pi.y, pl.z ) - R_ * Eigen::Vector3d 


( p2.x, pX.y, p2.z Jj 


40 // convert to cv::Mat 

4l R = ( Mat «double»(3,3) << 

42 R 00,0), RR 00,1), R 00,90. 

43 RH; E (ale BZ), 

44 5.42.0), BOs, R 

45 Ji 

46 t [Mat «doublas63. D) << & (0,00. ct CL.OD, E (9,0) 4 
47 |} 


ICP 的 实现 方式 和 前 文 讲述 的 是 一 致 的 。 我 们 调用 Eigen 进 行 SVD， 
然后 计算 Rt 窍 阵 。 我 们 输出 了 匹配 后 的 结果 ， 不 过 请 注意 ， 由 于 前 面 
的 推导 是 按照 p, =Rp ,+t 进行 的 ， 这 里 的 Rt E28 — p ll WI eR, 
与 前 面 PnP 部 分 是 相反 的 。 所 以 在 输出 结果 中 ， 我 们 同时 打 纯 了 人 敢 变 
换 : 


1 | 4 build/pose estimation, 3d3d 1.png 2.png 1 depth.png 2 depth.png 
2 |-- Max dist : 95.000000 

3 |-- Min dist : 4.000000 

4 | 一 共 找 到 了 79 组 匹配 点 

s |3d-3d pairs: 74 

6 |W= 298.51 -14.1815 41.0456 

7 |-44.8208 107.825 -164.404 

s |78.1978 -163.954 271.439 

9 |U= 0.474143 -0.880373 -0.0114952 

10 |-0.460275 -0.258979 0.849163 

1 |0.750556 0.397334 0.528006 

2 |V= 0.535211 -0.844064 -0.0332488 

13 |-0.434767 -0.309001 0.84587 

14 |0.724242 0.438263 0.532352 

i5 | ICP via SVD results: 

16 |R = [0.9972395976914055, 0.05617039049497474, -0.04855998381307948; 
17 | -0.05598344580804095, 0.9984181433274515, 0.005202390798842771; 
is | 0.04877538920134394, -0.002469474885032297, 0.998806719591959] 

i9 |t = [0.7086246277241892; 

2 | -0.2775515782948791; 

3 |-0.1559573762377209] 

2 |R inv = [0.9972395976914055, -0.05598344580804095, 0.04877538920134394; 


23 | 0.05617039049497474, 0.9984181433274515, -0.002469474885032297; 
24 | -0.04855998381307948, 0.005202390798842771, 0.998806719591959] 
2; | t. inv = [-0.7145999506834847 ; 

26 | 0.23692367 66013986; 

7 |0.1916260075851286] 





读者 可 以 比较 一 下 ICP 与 PpP、 对 极 几 何 的 运动 估计 结果 之 间 的 差 
异 。 可 以 认为 ， 在 这 个 过 程 中 我 们 使 用 了 越 来 越 多 的 信息 《没有 深度 一 
有 一 个 图 的 深度 一 有 两 个 图 的 深度 ) ， 因 此 ， 在 深度 准确 的 情况 下 ， 得 
到 的 估计 也 将 越 来 越 准 确 。 但 是 ， 由 于 Kinect 的 深度 图 存在 噪声 ， 而 且 
有 可 能 存在 数据 丢失 的 情况 ， 使 得 我 们 不 得 不 丢 径 一 些 没 有 深度 数据 的 
特征 点 。 这 可 能 导致 ICP 的 估计 不 够 准确 ， 并 有 旦 ， 如 果 特 征 点 丢弃 得 太 
多 ， 可 能 引起 由 于 特征 点 太 少 ， 无 法 进行 运动 估计 的 情况 。 


7.10.2” 非 线性 优化 方法 


下 面 考 虑 用 非 线 性 优化 来 计算 ICP。 我 们 依然 使 用 李 代 数 来 表达 相 
机 位 姿 。 与 SVD 思 路 不 同 的 地 方 在 于 ， 在 优化 中 我 们 不 仅 考虑 相机 的 位 
姿 ， 同 时 会 优化 3D 点 的 空间 位 置 。 对 我 们 来 说 ，RGB-D 相 机 每 次 可 以 
观测 到 路 标点 的 三 维 位 置 ， 从 而 产生 一 个 3D 观 测 数据 。 不 过 ， 由 于 
g&2o/sba 中 没有 提供 3D 到 3D 的 边 ， 而 我 们 又 想 使 用 g2o/sba 中 李 代 数 实现 
的 位 姿 节 点 ， 所 以 最 好 的 方式 是 和 目 定 义 一 种 这 样 的 边 ， 并 同 g2o 提 供 解 
析 求 导 方式 。 


四 slambook/ch7/pose estimation 3d3d.cpp 
1 | class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, 


g20: : VertexSE3Expmap> 
{ 


2 

3 | public: 

4 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 

5 EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : 

6 _point(point) 1) 

7 

8 virtual void computeError() 

9 { 

10 const g20::VertexSE3Expmap* pose = static_cast<const g2o0::VertexSE3Expmap*> 
( _vertices[0] ); 

11 // measurement is p, point is p' 

12 _error = measurement - pose->estimate().map( point ) ; 

13 } 

14 

15 virtual void linearizeÜplus() 

16 1 


17 g20::VertexSE3Expmap* pose = static cast«g20::VertexSE3Expmap *»( vertices 


[0]) ; 


18 g20::SE3Quat T(pose->estimate() ) ; 
19 Eigen::Vector3d xyz trans = T.map( point); 
20 double x = xyz trans[0]; 

21 double y = xyz trans[1]; 

22 double z = xyz trans[2]; 

23 

24 _jacobianOplusxi(0,0) = 0; 

25 _jacobian0Oplusxi(0,1) = -z; 

26 _jacobianOplusxi(0,2) = y; 

27 _jacobianOplusxi(0,3) = -1; 

28 _jacobianOplusxi(0,4) = 0; 

29 .jacobianOplusXi(0,5) = 0; 

30 

31 _jacobian0plusxi(1,0) = z; 

32 -.jacobianOplusXi(1,1) = 0; 

33 _jacobianOplusxXi(1,2) = -x; 

34 _jacobian0plusXi(1,3) = 0; 

35 _jacobian0plusXi(1,4) = -1; 

36 _jacobian0plusXi(1,5) = 0; 

37 

38 _jacobianOplusxi(2,0) = -y; 

39 .jacobianOplusXi(2,1) = x; 

40 _jacobianOplusxi(2,2) = 0; 

41 _jacobian0plusXi(2,3) = 0; 

42 _jacobian0plusXi(2,4) = 0; 

43 _jacobian0OplusxXi(2,5) = -1; 

44 } 

45 

46 bool read ( istream& in ) {} 

47 bool write ( ostream& out ) const {} 


48 | protected: 
49 Eigen: :Vector3d _point; 


$13; 


这 是 一 个 一 元 边 ， 写 法 类 似 于 前 面 提 到 的 
g20::EdgeSE3ProjectXYZ， 不 过 观测 量 从 2 维 变 成 了 3 维 ， 内 部 没有 相机 
模型 ， 并 且 只 关联 到 一 个 市 态 。 请 读者 注意 这 里 雅 可 比 和 矩阵 的 书写 ， 它 


必须 与 我 们 前 面 的 推导 一 致 。 雅 可 比 和 矩阵 给 出 了 关于 相机 位 姿 的 导数 ， 
是 一 个 3x 6 的 矩阵 。 

调用 g2o 进 行 优化 的 代码 是 相似 的 ， 我 们 设 定 好 图 优化 的 节点 和 边 
则 可 。 这 部 分 代码 请 读者 查看 源 文 件 ， 这 里 不 再 列 出 。 现 在 ， 来 看 看 优 
化 的 结果 : 





1 | calling bundle adjustment 
2 |iteration- 0 chi2= 452884.696837 time- 3.8443e-05 cumTime= 3.8443e-05  edges- 74 


schur- 0 

3 | iteration- 1 chi2- 452762.638918 time- 1.436e-05 cumTime= 5.2803e-05 edges= 74 
schur- 0 

4 | iteration- 2 chi2- 452762.618632 time- 1.1943e-05 

B ere "P jg ss 

6 |iteration- 9 chi2- 452762.618615 time- 1.0772e-05 cumTime- 0.000140108 edges- 74 
schur= 0 


7 | optimization costs time: 0.000528066 seconds. 


9 | after optimization: 

10 | T= 

1 |0.99724 0.0561704 -0.04856 0.708625 

12 | -0.0559834 0.998418 0.00520239 -0.277551 
i3 |0.0487754 -0.00246948 0.998807 -0.155957 
14 |O 0 0 1 





RIEHL, RIB UU DRAM AEDS, WHAEA 
EN Za BA CS. NVR aE RA DA, CAMA SVD 
dE 25 RI LA RE, OU SVD Zee hH SC e e ET 
解 。 所 以 ， 本 实验 中 可 以 认为 SVD 给 出 的 结果 是 相机 位 姿 的 最 优 值 。 


项 要 说 明 的 是 ， 在 本 例 的 ICP 中 ， 我 们 使 用 了 在 两 个 图 部 有 深度 读 
数 的 特征 点 。 然 而 事实 上 ， 只 要 其 中 一 个 图 深度 确定 ， 我 们 束 能 用 类 似 
于 PnP 的 误差 方式 ， 把 它们 也 加 到 优化 中 来 。 同 时 ， 除 了 相机 位 姿 之 
外 ， 将 空间 点 也 作为 优化 变量 考虑 ， 亦 是 一 种 解决 问题 的 万 式 。 我 们 应 
当 清 杷 ， 实 际 的 求解 是 非常 郝 活 的 ， 不 作 拘 泥 于 霖 种 固定 的 形式 。 如 琳 
间 时 知 夸 点 和 相机 ， 荆 个 问题 融 变 得 更 目 由 了， 你 可 能 会 得 到 其 他 的 


解 。 比 如 ， 可 以 让 相机 少 转 一 些 角 度 ， 而 把 点 多 移动 一 些 。 这 从 男 一 侧 
面 有 反映 出 ， 在 Bundle Adjustment 里 面 ， 我 们 会 布 望 有 尺 可 能 多 的 约束 ， 
因为 多 次 观测 会 市 来 更 多 的 信息 ， 使 我 们 能 够 更 准确 地 佑 计 每 个 变量 。 


7.11 小 结 


本 讲 介绍 了 基于 特征 点 的 视觉 里 程 计 中 的 几 个 重要 的 问题 。 包 括 : 

1. 特 征 点 是 如 何 提取 并 匹配 的 。 

2. 如 何 通 过 2D-2D 的 特征 点 估计 相机 运动 。 

3. 如 何 从 2D-2D 的 匹配 佑 计 一 个 点 的 空间 位 置 。 

4.3D-2D 的 PnP 问题 ， 其 线性 解法 和 Bundle Adjustment 解 法 。 

5.3D-3D 的 ICP 回 题 ， 其 线性 解法 和 Bundle Adjustment 解 法 。 

本 讲 内 容 较为 丰 吝 ， 且 绪 合 应 用 了 前 几 讲 的 基本 知识 。 旋 者 硬 和 觉得 
理解 有 困难 ， 可 以 对 前 面 知 识 和 加 回顾 。 最 好 亲自 做 一 授 实 验 ， 以 理解 
整个 运动 估计 的 内 容 。 

需要 解释 的 是 ， 为 你 证 行文 流畅 ， 我 们 和 省略 了 大 量 天 于 菜 些 特殊 情 
况 的 讨论 。 例 如 ， 如 末 在 对 极 几 何 求解 过 程 中 给 定 的 特征 点 共 面 ， 会 及 
生 什 么 情况 〈 这 在 单 应 矩阵 五 中 提 到 了 ) ? 共 线 又 会 肥 生 什么 情况 ? 在 
PnP 和 ICP 中 大 给 定 这 样 的 解 ， 又 会 导致 什么 情况 ? 求解 算法 能 售 识 询 这 
些 特殊 的 情况 ， 并 报告 所 得 的 解 可 能 不 可 靠 ? 尽管 它 们 都 是 值得 研究 和 
探索 的 ， 然 而 对 它们 的 讨论 势 作 让 本 书 变 得 特别 烦琐 。 而 且 在 工程 实现 
中 ， 这 些 情况 其 少 出 现 ， 所 以 本 书 介 绍 的 方法 ， 是 指 在 实际 工程 中 能 够 
有 效 运行 的 方法 ， 我 们 假定 了 那些 少见 的 情况 并 不 发 生 。 如 采 你 关心 这 
些 少 见 的 情况 ， 可 以 阅读 [3] 等 论文 ， 在 文献 中 我 们 经 常会 研究 一 些 特殊 
情况 下 的 解雇 方案 。 

习题 

1. 除 了 本 书 介 绍 的 ORB 特 征 点 外 ， 你 还 能 找到 哪些 特征 点 ? 请 说 说 
SIFT 或 SURF 的 原理 ， 并 对 比 它 们 与 ORB 之 间 的 优 务 。 


2. 设 计 程 序 调用 OpenCV 中 的 其 他 种 类 特征 点 。 统 计 在 提取 1000 个 
特征 点 时 在 你 的 机 右上 所 用 的 时 间 。 


3.* 我 们 发 现 ，OpenCV 提 供 的 ORB 特 征 点 在 图 像 当中 分 布 不 够 均 


勺 。 你 是 否 能 够 找到 或 提出 让 特征 点 分 布 更 加 均匀 的 方法 ? 

4. 研 究 FLANN 为 何 能 够 快速 处 理 匹 配 问 题 。 除 了 FLANN 之 外 ， 还 
有 哪些 可 以 加 速 匹 配 的 手段 ? 

5. 把 演示 程序 使 用 的 EPnP 改 成 其 他 PnP 方 法 ， 并 研究 它们 的 工作 原 
Bs 

6. 在 PnP 优化 中 ， 将 第 一 个 相机 的 观测 也 考虑 进来 ， 程 序 应 如 何 书 
5? 最 后 结果 会 有 何人 变化 ? 

7. 在 ICP 程 序 中 ， 将 空间 点 也 作为 优化 变量 考虑 进来 ， 程 序 应 如 何 
书写 ? 最 后 结果 会 有 何人 变化 ? 

8.* 在 特征 点 匹配 过 程 中 ， 不 可 避免 地 会 遇 到 误 匹 配 的 情况 。 如 果 
我 们 把 错误 匹配 输入 到 PnP 或 ICP 中 ， 会 发 生 怎 样 的 情况 ?你 能 想到 哪些 
避免 误 匹 配 的 方法 ? 

9.* 使 用 Sophus 的 SE3 类 ， 目 己 设 计 g20 的 市 点 与 边 ， 实 现 PnP 和 ICP 
的 优化 。 

10.* 在 Ceres 中 实现 PnP 和 ICP 的 优化 。 





[1] 金字 塔 是 指 对 图 像 进行 不 同 层次 的 降 采 样 ， 以 获得 不 同 分 辩 率 的 图 像 。 
[2] 也 就 是 说 ， 在 等 式 一 侧 乘 以 任意 非 零 第 数 时 ， 我 们 认为 等 式 仍 是 成 并 的 。 
[3] 在 SfEM 研 究 中 则 有 可 能 是 未 知 而 有 竺 估计 的 。 

[4] 请 注音， 这 和 SE(3) 中 的 变换 矩阵 T 古 不 同 的 。 


[5] exp(é ^ )P ,结果 是 4x 1 的 ， 而 其 左 侧 的 K 是 3x 3 的 ， 所 以 必须 把 exp(E^ )P ,的 前 三 维 取出 来 ， 
变 成 三 维 的 非 齐 次 坐标 。 这 在 前 面 讲 过 。 
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主要 目标 

1. 理 解 光 流 法 跟踪 特征 点 的 原理 。 

2. 理 解 下 接 法 是 如 何 佑 计 相 机 位 姿 的 。 

3. 使 用 g20 进 行 卫 接 法 的 计算 。 

十 接 法 是 视 党 里 程 计 万 一 主要 分 文 ， 它 与 特征 点 法 有 很 大 不 同 。 虽 
然 它 还 没有 成 为 现在 VO 中 的 主流 ， 但 经 过 近 几 年 的 发展 ， 下 接 法 在 一 
定 程度 上 已 经 能 和 特征 点 法 平分 秋色 。 本 讲 我 们 将 介绍 直接 法 的 原理 ， 
并 利用 g2o 实 现下 接 法 中 的 一 些 核心 算法 。 











in | Tp- J, | KRptt)) I, 


ol ow ou 
J= Sr 3% 33 


= -j 


8.1 ARYAN SI E 


上 一 讲 我 们 介绍 了 使 用 特征 点 估计 相机 运动 的 方法 。 尽 管 特征 点 法 
在 视 党 里 程 计 中 占据 主流 地 位 ， 但 研究 者 们 还 是 认识 到 它 至 少 有 以 下 几 
个 缺点 : 

1. 天 键 点 的 提取 与 接 述 子 的 计算 非常 耗 时 。 实 践 当 中 ，SIFT 目 前 在 
CPU 上 是 无 法 实时 计算 的 ， 而 ORB 也 需要 近 20ms 的 计算 。 如 果 整 个 
SLAM 以 30 坚 秒 / 顺 的 速度 运行 ， 那 么 一 大 半 时 间 都 将 花 在 计算 特征 点 
pes 


2. 使 用 特征 点 时 ， 忽 上 略 了 除 特 征 点 以 外 的 所 有 信息 。 一 幅 图 像 有 几 
十 万 个 像素 ， 而 特征 点 只 有 几 百 个 。 只 使 用 特征 点 丢弃 了 大 部 分 可 能 
用 的 BEER E E o 


3. 相 机 有 时 会 运动 到 特征 缺失 的 地 方 ， 这 些 地 方 往 往 没有 明显 的 级 
理 信息 。 例 如 ， 有 时 我 们 会 面 对 一 霹 白 墙 ， 或 者 一 个 空 汤 荡 的 走廊 。 这 
些 场 景 下 特征 点数 量 会 明显 减少 ， 我 们 可 能 找 不 到 足够 的 匹配 点 来 计算 
相机 运动 。 

我 们 看 到 使 用 特征 点 确实 存在 一 些 问 题 。 有 没有 什么 办 法 能 够 元 服 
这 些 缺 点 呢 ? 我 们 有 以 下 几 种 中路: 


保留 特征 点 ， 但 只 计算 关键 点 ， 不 计算 描述 子 。 同 时 ， 使 用 光 流 
ik (OpticalFlow) 来 跟踪 特征 点 的 运动 。 这 样 可 以 回避 计算 和 匹配 描 
述 子 寓 来 的 时 间 ， 但 欧洲 本 有 身 的 计算 需要 一 定时 间 。 

。 只 计算 天 键 点 ， 不 计算 摘 述 子 。 同 时 ， 使 用 直接 法 (Direct 
Method) 来 计算 特征 点 在 下 一 时 刻 图 像 中 的 位 置 。 这 同样 可 以 跳 过 摘 述 
子 的 计算 过 程 ， 而 且 直 接 法 的 计算 更 加 简单 。 

* 婚 不 计算 关键 点 ， 也 不 计算 描述 子 ， 而 是 根据 像素 灰 度 的 差异 ， 
直接 计算 相机 运动 。 

第 一 种 方法 仍然 使 用 特征 点 ， 只 是 把 匹配 摘 述 子 蔡 换 成 了 光 流 跟 
蹊 ， 估 计 相 机 运动 时 仍 使 用 对 极 几 何 、PnP 或 ICP 算 法 。 而 在 后 两 种 方法 
中 ， 我 们 会 根据 网 像 的 像素 灰 度 信息 “来 计算 相机 运动 ， 它 们 都 称 为 直 


接 法 。 


使 用 特征 点 法 估计 相机 运动 时 ， 我 们 把 特征 点 看 作 回 定 在 三 维 空间 
的 不 动 点 。 根 据 它 们 在 相机 中 的 投影 位 置 ， 通 过 最 小 化 重 投 影 误 关 
(Reprojection error) 来 优化 相机 运动 。 在 这 个 过 程 中 ， 我 们 需要 精确 
地 知道 空间 点 在 两 个 相机 中 投影 后 的 像 系 位 置 一 一 这 也 束 是 我 们 要 对 特 
征 进 行 死 配 或 跟踪 的 原因 。 同 时 ， 我 们 也 知道 ， 计 算 、 匹 配 特 征 需要 付 
出 大 量 的 计算 量 。 相 对 地 ， 在 直接 法 中 ， 我 们 并 不 需要 知道 点 与 点 之 间 
WX VARA, Tata) tim (Photometric error) 来 求 得 它 
(ek 

直接 法 是 本 讲 介 绍 的 重点 。 它 是 为 了 克服 特征 点 法 的 上 述 缺 点 而 存 
在 的 。 生 接 法 根据 像 辫 的 完 度 信息 估计 相机 的 运动 ， 可 以 完全 不 用 计算 
关键 点 和 摘 述 和子， 于 是 ， 既 避免 了 特征 的 计算 时 间 ， 也 避免 了 特征 缺失 
的 情况 。 只 要 场景 中 存在 明暗 变化 (可 以 是 渐变 ， 不 形成 局 部 的 图 像 柳 
E), HRAMBE LIFE. IWARA, BRENA ME 
AUF Aid 2 — Rh. RE FRIERE R EWART OPA). H 
PRIA ALA cs qu as BMAF Hd uz PS YY Be 


历史 上 ， 早 期 也 有 对 和 直接 法 的 使 用 59 ”。 随 看 一 些 使 用 和 下 接 法 的 开 
源 项 日 的 出 现 (如 SVOS59 、LSD-SLAMI7 等 ) ， 它 们 逐渐 地 走 上 主流 
舞台 ， 成 为 视 党 里 程 计 算法 中 重要 的 一 部 分 。 


8.2 tù (OpticalFlow) 


直接 法 是 从 光 流 渤 变 而 来 的 。 它 们 非常 相似 ， 有 具有 相同 的 假设 条 
件 。 交 法 接 述 了 像 系 在 图 像 中 的 运动 ， 而 直接 法 则 附带 看 一 个 相机 运动 
模型 。 为 了 说 明和 直接 法 ， 我 们 先 来 介绍 一 下 交流。 

光 流 是 一 种 摘 述 像 系 随时 间 在 图 像 之 则 运动 的 方法 ， 如 图 8-1 所 
示 。 随 看 时 间 的 流逝 ， 同 一 个 像 头 会 在 图 像 中 运动 ， 而 我 们 布 望 退 踩 它 
的 运动 过 程 。 其 中 ， 计 算 部 分 像素 运动 的 称 为 稀 玖 光 流 ”， 计 算 所 有 像 
AWE AA Sti 。”。 稀 下 光 流 以 Lucas-Kanade 光 流 为 代表 ， 并 可 以 在 
SLAM 中 用 于 跟 踊 符 征 点 位 置 。 因 此 ， 本 节 主 要 介绍 Lucas-Kanade 光 
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图 8-1 LK 交流 法 示意 图 。 


Lucas-Kanade?c yi 


在 LK 交流 中 ， 我 们 认为 来 自 相 机 的 图 像 古 随时 间 变 化 的 。 图 像 可 
以 看 作 时 间 的 函数 : I(t )。 那 么 ， 一 个 在 t 时 刻 ， 位 于 (xy ) 处 的 像 系 ， 
它 的 灰 度 可 以 写成 


Itn. 


JUR SUE EUR X RT A-SI TRU] BAS EEN ea ce ER 
HAR ARN ARS DUES SR TERIAL, CEt ”时刻 的 像 系 坐标 


Axy  。 由 于 相机 的 运动 ， 它 的 图 像 坐 标 将 发 生变 化 。 我 们 希望 估计 这 
个 空间 点 在 其 他 时 刻 图 像 中 位 置 。 怎 么 估计 呢 ? 这 里 要 引入 光 流 法 的 基 
本 假设 。 

灰 度 不 变 假设 ”: 同一 个 空间 点 的 像素 灰 度 值 ， 在 各 个 图 像 中 是 回 
定 不 变 的 。 

对 于 t 时 刻 位 于 (x,y ) 处 的 像素 ， 我 们 设 t +dt 时 刻 它 运动 到 (x +dx,y 
+dy) 处 。 由 于 灰 度 不 变 ， 我 们 有 : 


I(x + dz,y + dy,t + dt) = I(x,y,t). (8.1) 


灰 度 不 变 假 设 是 一 个 很 强 的 假设 ， 实 际 当 中 很 可 能 不 成 芯 。 事 实 
上 ， 由 于 物体 的 材质 不 同 ， 像 素 会 出 现 高 光 和 阴影 部 分 ， 有 时 ， 相 机 会 
自动 调整 曝光 参数 ， 使 得 图 像 整 体 变 亮 或 变 暗 。 这 些 时 候 灰 度 不 变 假 设 
都 是 不 成 立 的 ， 因 此 光 流 的 结果 也 不 一 定 可 靠 。 然 而 ， 从 另 一 方面 来 
说 ， 所 有 算法 都 是 在 一 定 假 设 下 工作 的 。 如 果 我 们 什么 假设 都 不 做 ， 就 
没 法 设计 实用 的 算法 。 所 以 ， 让 我 们 暂且 认为 该 假设 成 这 ， 看 看 如 何 计 
算 像 系 的 运动 。 

对 左边 进行 泰勒 展开 ， 保 留 一 阶 项 ， 得 : 


I I I 
I (x + dz,y + dy,t + dt) ~ I(az,y,t) + vs ar 十 PN 十 9 dt (8.2) 
Ox Oy Ot 


AAI MB FARBER BR, Tæ BPAY ZU ARES ZB ZR 
B, 从 而 : 


OI OI OI 


两 边际 以 dt fa: 
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ürdt  yds BE (8.4) 


其 中 dx /dt 为 像 系 在 x 轴 上 的 运动 速度 ， 而 dy /dt Ay 轴 上 的 速度 ， 


把 它们 记 为 uv 。 同 时 8T /0x 为 图 像 在 该 点 处 x 方向 的 梯度 ， 另 一 项 则 是 
在 y 方向 的 梯度 ， 记 为 I, ,I[，。 把 图 像 灰 度 对 时 间 的 变化 量 记 为 1 ， 写 成 
矩阵 形式 ， 有 : 


[LOL 四 = -h (8.5) 


我 们 力 计 算 的 是 像 妹 的 运动 wy ， 但 是 该 式 是 市 有 两 个 变量 的 一 次 
方程 ， 仅 赁 它 无 法 计算 出 wy 。 因 此 ， 必 须 引 入 额外 的 约束 来 计算 wy 。 
在 LK 交流 中 ， 我 们 假设 茶 一 个 禄 口内 的 像 系 其 有 相同 的 运动 。 

考虑 一 个 大 小 为 wxw 的 窗口 ， 它 舍 有 w ”数量 的 像 和 水。 由 于 该 窗口 
内 像素 具有 同样 的 运动 ， 因 此 我 们 共有 w ?个 方程 : 


u 
| I; Iy | | | = — gps k=1,..., w*. (8.6) 
k U 
W: 
p Eyl; Iu 
A= : bal = |}. (8.7) 
H.E |. Ii 
于 是 整个 方程 为 
U 
A _ (8.8) 
U 








这 是 一 个 天 于 wv 的 超 定 线性 方程 ， 传 统 解法 是 求 好 小 二 来 解 。 最 


小 二 乘 在 很 多 时 候 都 用 到 过 : 


| : | = - (ATA) ATb. (8.9) 

这 样 就 得 到 了 像素 在 图 像 间 的 运动 速度 wv 。 当 t 取 离 散 的 时 刻 而 不 
是 连续 时 间 时 ， 我 们 可 以 估计 某 块 像 率 在 硅 干 个 图 像 中 出 现 的 位 置 。 由 
于 像 双 梯度 仪 在 局 部 有 效 ， 所 以 如 果 一 次 从 代 不 够 好 ， 我 们 会 多 连 代 几 
次 这 个 方程 。 在 SLAM 中 ，LK 光 流利 被 用 来 跟踪 角 点 的 运动 ， 我 们 不 妨 
通过 程序 体会 一 下 。 


8.3 E: LKE 


8.3.1 ”使 用 TUM 公 开 数 据 集 


下 面 演示 如 何 用 OpenCV 提 供 的 光 流 法 来 跟踪 特征 点 。 与 上 一 节 一 
样 ， 我 们 准备 了 若干 幅 数 据 集 图 像 ， 存 放 在 程序 目录 中 的 data/ 文 件 夹 

下 。 它 们 来 自 于 慕尼黑 工业 大 学 (TUM) 提供 的 公开 RGB-D 数 据 集中 

， 以 后 我 们 就 称 之 为 TUM 数 据 集 。 它 含有 许多 个 RGB-D 视 频 ， 可 以 作 
为 RGB-D 或 单 目 SLAM 的 实验 数据 。 它 还 提供 了 用 运动 捕捉 系统 测量 的 
精确 轨迹 ， 可 以 作为 标准 轨迹 以 校准 SLAM 系 统 。 由 于 该 数据 集 比较 

大 ， 我 们 没有 放 到 GitHub 上 (否则 下 载 代码 需要 很 长 时 间 ) ， 请 读者 自 
行 去 数据 集 主页 查找 对 应 的 数据 。 本 程序 中 使 用 了 一 部 

分 “freburg1_desk” 数 据 集中 的 图 像 。 读 者 可 以 在 TUM 数 据 集 主页 找到 它 
的 下 载 链接 。 或 者 ， 也 可 以 直接 使 用 本 书 在 GitHub 上 提供 的 部 分 。 


我 们 的 数据 位 于 本 章 目 录 的 data/ 下， 以 压缩 包 形 式 提 供 
(data.tar.gz) 。 由 于 TUM 数 据 集 是 从 实际 环境 中 采集 的， 需要 解释 一 
下 它 的 数据 格式 《数据 集 一 般 都 有 目 己 定义 的 格式 ) 。 在 解压 后 ， 你 将 
看 到 以 下 这 些 文件 : 

1.rgb.txt 和 depth.txt 记 录 了 各 文件 的 采集 时 间 和 对 应 的 文件 名 。 

2.rgb/ 和 depth/ 目 录 存 放 痢 采集 到 的 PNG 格 式 图 像 文件 。 彩 色 图 像 为 
8 位 3 通道 ， 深 上 度 图 为 16 位 单 通道 图 像 。 文 件 名 即 采 集 时 间 。 

3.groundtruth.txt 为 外 部 运动 捕捉 系统 采集 到 的 相机 位 姿 ， 格 式 为 

(E, bans Fontans D Ga an 

我 们 可 以 把 它 看 成 标准 轨迹 。 

请 注意 ， 彩 色 图 、 深 友 图 和 标准 轨迹 的 采集 都 是 独立 的 ， 轨 迹 的 玉 
集 频 率 比 图 像 蜗 很 多 。 在 使 用 数据 之 前 ， 需 要 根据 采集 时 间 对 数据 进行 
一 次 时 间 上 的 对 齐 ， 以 便 对 彩色 图 和 深度 图 进行 配对 。 原 则 上 ， 我 们 可 
以 把 采集 时 间 相 近 于 一 个 国 值 的 数据 ， 看 成 是 一 对 疼 像 。 并 把 相近 时 间 


的 位 姿 ， 看 作 是 该 图 像 的 真实 采集 位 置 。TUM 提 供 了 一 个 Python 脚 
本 “associate.py”( 或 使 用 slambookytoolsassociate.py) 大 我 们 完成 这 件 
事 。 请 把 此 文件 放 到 数据 集 目 录 下 ， 运 行 : 


1 | Python associate.py rgb.txt depth.txt > associate.txt 


这 上 段 脚 本 会 根据 输入 的 两 个 文件 中 的 采集 时 间 进 行 配对 ， 最 后 输出 
到 文件 associate.txt。 输 出 文件 人 台 有 配对 后 的 两 幅 图 像 的 时 间 、 文 件 名 售 
晨 ， 可 以 作为 后 续 处 理 的 来 源 。 此 外 ，TUM 数 据 集 还 提供 了 比较 估计 
轨迹 与 标准 轨迹 的 工具 ， 我 们 将 在 用 到 的 地 方 再 进行 介绍 。 


8.3.2 ”使 用 LK 交流 


下 面 来 编写 程序 使 用 OpenCV 中 的 LK 光 流 。 使 用 LK 的 日 的 是 跟踪 
特征 点 。 我 们 对 第 一 幅 图 像 提 取 FAST 角 点 ， 然 后 用 LK 光 流 跟踪 它们 ， 
FEMER F. 


k) slambook/ch8/useLK/useLK.cpp 





#include <iostream> 


#include <fstream> 
#include <list> 
#include <vector> 
#include <chrono> 


using namespace std; 


~] oO un I və N _ 


#include 
#include 
#include 


#include 


<opencv2/core/core.hpp> 
<opencv2/highgui/highgui . hpp> 
<opencv2/features2d/features2d.hpp> 
<opencv2/video/tracking.hpp> 


int main( int argc, char** argv ) 


{ 
1f * 
1 


} 


argc != 2) 


cout««"usage: useLK path to dataset"««endl; 


return 1; 


string path to dataset = argv[1]; 


string associate file = path to dataset + "/associate.txt"; 


ifst 


ream fin( associate file ); 


string rgb file, depth file, time rgb, time depth; 


list 
Cv: 
for 


1 


< cv::Point2f > keypoints; // 因为 要 删除 跟踪 失败 的 点 ,使 用 1ist 


Mat color, depth, last color; 


( int index-0; index«100; index++ ) 


fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 
color = cv::imread( path_to_dataset+"/"+rgb_file ); 
depth = cv::imread( path_to_datasett+"/"+depth_file, -1 ); 
if (index ==0 ) 
{ 
// 对 第 一 帧 提取 FAST 特征 点 


Vector<cV: :KeyPoint> kps; 


cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector:: 


create(); 
detector->detect( color, kps ); 
for ( auto kp:kps ) 
keypoints.push, back( kp.pt ); 
last color = color; 
continue; 
} 
if ( color.data==nullptr || depth.data==nullptr ) 
continue; 
// 对 其 他 帧 用 LK 跟踪 特征 点 
vector<cv::Point2f> next keypoints; 
vector<cv::Point2f> prev keypoints; 
for ( auto kp:keypoints ) 
prev keypoints.push, back(kp); 
vector«unsigned char»? status; 


vector<float> error; 


chrono::steady clock::time point t1 = chrono::steady clock::now(); 


52 cv::calcOpticalFlowPyrLK( last color, color, prev keypoints, next keypoints 


, Status, error ); 


53 chrono::steady clock::time point t2 = chrono::steady clock::now(); 

54 chrono::duration<double> time used = chrono::duration cast«chrono::duration 
<double>>( t2-t1 ); 

55 cout<<"LK Flow use time: "<<time_used.count()<<" seconds."««endl; 

56 // 把 跟 丢 的 点 删 皖 

57 int i=0; 

58 for ( auto iter-keypoints.begin(); iter!-keypoints.end(); i++) 

59 { 

60 if ( status[i] == 0 ) 

él 1 

62 iter = keypoints.erase(iter); 

63 continue; 

64 

65 xiter = next keypoints[i]; 

66 iter*t*; 

67 } 

68 cout<<"tracked keypoints: "<<keypoints.size()<<end1; 

69 if (keypoints.size() == 0) 

70 { 

71 cout<<"all keypoints are lost."««endl; 

72 break; 

73 } 

74 // Š ŒE keypoints 

75 cv::Mat img show = color.clone(); 

76 for ( auto kp:keypoints ) 

77 cv::circle(img show, kp, 10, cv::Scalar(0, 240, 0), 1); 

78 cv::imshow("corners", img show); 

79 cv: :waitKey (0) ; 

80 last_color = color; 

81 } 

82 return 0; 

ME. 


读者 应 当 已 经 熟悉 了 OpenCV 的 使 用 方式 ， 这 里 就 不 提供 
CMakeLists.txt 的 写法 了 。 SEPN 数据 集 所 在 的 月 


孙 ， 例如 : 
| 


我 们 会 在 每 次 循环 后 暂 仿 程序 ， 按 任意 键 可 以 继续 运行 。 你 会 看 到 
图 像 中 大 部 分 特征 点 部 能 够 顺利 跟 躁 到 ， 但 也 有 特征 点 会 去 失 。 丢 失 的 
和 翌 征 点 或 是 个 移出 了 视野 外 ， 或 是 个 其 他 物体 描 住 了 。 如 末 我 们 不 提取 
新 的 特征 点 ， 那 么 区 流 的 跟踪 会 越 来 越 少 : 


^ build/uselK ../data 

LK Flow use time: 0.0329535 seconds. 
tracked keypoints: 1749 

LK Flow use time: 0.0247758 seconds. 
tracked keypoints: 1742 

LK Flow use time: 0.0226143 seconds. 
tracked keypoints: 1703 

LK Flow use time: 0.0238692 seconds. 
tracked keypoints: 1676 

LK Flow use time: 0.0210466 seconds. 
tracked keypoints: 1664 

LK Flow use time: 0.0226533 seconds. 
tracked keypoints: 1656 

LK Flow use time: 0.0266527 seconds. 
tracked keypoints: 1641 

LK Flow use time: 0.0214207 seconds. 
tracked keypoints: 1634 





图 8-2 显 示 了 程序 运行 过 程 中 右 干 帧 的 情况 (这 里 使 用 了 完整 的 数 
据 集 ， 但 本 书 的 git 上 只 给 出 了 10 张 图 ) 。 最 初 我 们 大 约 有 1700 个 特征 
扩 。 跟 躁 过 程 中 一 部 分 特征 点 会 丢失 ， 下 a 到 100 帕 时 我 们 还 有 约 178 个 特 
征 点 ， 相 机 视角 相对 于 最 初 的 图 像 也 及 生 了 较 大 改变 。 
































图 8-2 LKE o 


仔细 观察 特征 点 的 跟 踩 过 程 ， 我 们 会 肥 现 位 于 物体 角 点 处 的 特征 更 
加 稳定 。 亡 绿 处 的 特征 会 治 看 边缘 “请 动 ”， 这 主要 在 由 于 治 看 边 绿 移动 
时 特征 所 的 内 容 基 本 不 变 ， 因 此 程序 容易 认为 是 同一 个 地 方 。 而 既 不 
在 角 点 也 不 在 边缘 的 特征 氮 则 会 频 素 跳动 ， 位 置 非 钊 不 稳定 。 这 个 现象 
很 像 围棋 中 的 “ 金 角 银 按理 肚 诡 >”: ARRA RI, WEIR, 
区 块 最 少 。 

为 一 方 务 ， 读 者 可 以 看 到 交流 法 的 运行 时 间 。 在 跟 踊 1500 个 特征 后 
时 ，LK 交 流 法 大 约 需 要 20ms。 如 果 减 小 特征 点 的 数量 ， 则 会 明显 减少 
计算 时 间 。 我 们 看 到 ，LK 光 流 跟 踪 法 避免 了 描述 子 的 计算 与 匹配 ， 但 
本 吴 世 需要 一 定 的 计算 量 。 在 我 们 的 计算 平台 上 ， 使 用 LK 交 这 能 够 节 
省 一 定 的 计算 量 ， 但 在 具体 SLAM 系 统 中 ， 使 用 光 流 还 是 匹配 摘 述 子 ， 
最 好 古 杀 目 做 实验 测试 一 下 。 


态 外 ，LK 交 流 跟 躁 能 够 直接 得 到 特征 点 的 对 应 关系 。 这 个 对 应 关 
系 就 像 是 描述 子 的 匹配 ， 但 实际 上 我 们 大 多 数 时 候 只 会 碰 到 特征 点 跟 丢 
的 情况 ， 而 不 太 会 过 到 误 匹 配 ， 这 应 该 是 光 流 相对 于 接 述 子 的 一 扩 优 
劳 。 但 是 ， 匹 配 搓 述 子 的 方法 在 相机 运动 较 大 时 仍 能 成 功 ， 而 光 流 必须 


要 求 相机 运动 是 微小 的 。 从 这 方面 来 说 ， 光 法 的 健壮 性 比 摘 述 子 于 一 
些 。 


最 后 ， 我 们 可 以 通过 交流 跟 踩 的 特征 点 ， 用 PnP、ICP 或 对 极 几 何 来 
估计 相机 和 运动， 这 些 方法 在 上 一 讲 中 介绍 过 ， 这 里 不 再 讨论 。 和 总 而 言 
之 ， 交 流 法 可 以 加 速 基于 特征 点 的 视 党 里 程 计 算法 ， 避 免 计 算 和 匹配 描 
述 子 的 过 程 ， 但 要 求 相 机 运动 较 慢 《或 采集 频率 较 高 ) 。 


8.4 直接 法 (DirectMethod ) 


接 下 来 ， 我 们 来 讨论 与 光 流 有 一 定 相似 性 的 直接 法 。 与 前 面 内 容 相 
似 ， 我 们 移 介 绍 直 接 法 的 原理 ， 然 后 使 用 g2o 实 现 直 接 法 。 


8.1. 二 接 法 的 推导 


如 图 8-3 所 示 ， 考 虑 某 个 空间 点 P 和 两 个 时 刻 的 相机 。P 的 世界 坐标 
为 [XYZ ]， 它 在 两 个 相机 上 成 像 ， 记 非 齐 次 像素 坐标 为 p , ,p ，。 


我 们 的 目标 是 求 第 一 个 相机 到 第 二 个 相机 的 相对 位 姿 变换 。 我 们 以 
第 一 个 相机 为 参照 系 ， 设 第 二 个 相机 的 旋转 和 平移 为 Rt ”对 应 于 代 数 
JE ) 。 同 时 ， 两 相机 的 内 参 相 同 ， 记 为 K 。 为 清楚 起 见 ， 我 们 列 写 完 


整 的 投影 方程 ; 
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图 8-3 ”年 接 法 示意 图 。 
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其 中 2 , 是 P 的 深度 ，Z , P ERMENE AR PINAR, that 
是 RP +t 的 第 3 个 坐标 值 。 由 于 exp(E ) 只 能 和 齐 次 坐标 相 乘 ， 所 以 我 们 
乘 完 之 后 要 取出 前 3 个 元 素 。 这 和 上 一 讲 及 相机 模型 部 分 的 内 容 是 一 致 
的 。 

回忆 特征 点 法 中 ， 由 于 我 们 通过 匹配 摘 述 子 知道 Jp , ,p , PARM 


置 ， 所 以 可 以 计算 重 投影 的 位 置 。 但 在 直接 法 中 ， 由 于 没有 特征 匹配 ， 
我 们 无 从 知道 哪 一 个 p , 与 p ;对 应 看 同一 个 后 。 和 直接 法 的 思路 是 根据 当 亨 


相机 的 位 姿 估 计 值 来 寻找 p , 的 位 置 。 但 车 相机 位 姿 不 够 好 ，p , KIAMA 
p 会 有 明 暑 差别。 于是， 为 了 减 小 这 个 差别 ， 我 们 优化 相机 的 位 姿 ， 来 
寻找 与 p ,更 相似 的 p , 。 这 同样 可 以 通过 解 一 个 优化 问题 完成 ， 但 此 时 年 
小 化 的 不 是 重 投 影 误 产 ， 而 是 光 撒 误 产 (Photometric Error) ， 也 就 是 P 
E P] “MAR Su EIR : 
es Fy (p1) — [^ (p2). (8.10) 

注意 这 里 e 是 一 个 标量 。 同 样 地 ， 优 化 目标 为 该 误差 的 二 范 数 ， 蜀 

时 取 不 加 权 的 形式 ， 为 


min J (£) = llell. (8.11) 


能 够 做 这 种 优化 的 理由 ， 仍 是 基于 灰 度 不 变 假设 。 在 卫 接 法 中 ， 
我 们 假设 一 个 空间 后 在 各 个 视角 下 成 像 的 灰 度 古 不 变 有 的 。 我 们 有 许多 个 


(比如 N 个) 空间 后 P; ， 那 么 ， 整 个 相机 位 姿 佑 计 问 题 变 为 


(£) — > elei, e; = I, (pii) = Iz (p23). (8.12) 
"m 


注意 这 里 的 优化 变量 是 相机 位 次 5 。 为 了 求解 这 个 优化 问题 ， 我 们 
Rib ine 是 如 何 随 看 相机 位 次 5 变化 的 ， 需 要 分 析 它 们 的 导数 关系 。 
因此 ， 使 用 李 代 数 上 的 扰动 模型 。 我 们 给 exp(E  ) 顽 乘 一 个 小 扰动 exp(66 


)， 得 : ES 
e(€@d0E) =; (KP) 一 了 2 (7K exp (E^) exp (£^) P) 
1 l 


1 1 1 
= J, (z.kP) — D (z, Kem (£^) P+ g K^ exp er) ! 


q = E^ exp (£^) P, 
1 


= — Kq. 
u Z, q 


这 里 的 q 为 扰动 分 量 在 第 二 个 相机 坐标 系 下 的 坐标 ， 而 u 为 它 的 像 
Ahh. MBAR, A: 


e(£oót)— Il (7-KP) 一 72 (z, Kew (£^) P+ u) 


oL (p) -r ( eig p) 252" og 
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我 们 看 到 ， 一 阶 导数 由 于 链 式 法 则 分 成 了 3 项 ， 而 这 3 项 都 是 容易 计 
算 的 : 


1.0I , /6u Nu 处 的 像 系 梯度 。 


2.0u /0g ”为 投影 方程 关于 相机 上 坐标 系 下 的 三 维 点 的 导数 。 记 q = 
[X,Y,Z ]， 根 据 上 一 节 的 推导 ， 导 数 为 








Ou u ðu i. fX 
Ou | ox ay oz |_| z 0 一 多 eis 
ðq ðv ðv ev 0 fu AY | 
OX OY OZ Z Z2 


3.0q /05E 为 变换 后 的 三 维 点 对 变换 的 导数 ， 这 在 李 代数 一 讲 介绍 过 
fi 


在 实践 中 ， 由 于 后 两 项 只 与 三 维 点 q 有 关 ， 而 与 图 像 无 关 ， 我 们 经 
音 把 它 合并 在 一 起 : 














» LA » » un 3 
ou [4 0 -各 -+ 
一 一 二 . (8.15) 
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这 个 2x 6 的 窍 阵 在 上 一 讲 中 也 出 现 过 。 于 是 ， 我 们 推导 出 误 甜 相对 
于 李 代 数 的 雅 可 比 窍 阵 : 
Ol; Ou 
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8.4.2 HARIT 


在 上 面 的 推 寻 中 ，P 是 一 个 已 知 位 置 的 空间 点 ， 它 古 怎 么 来 的 呢 ? 


在 RGB-D 相 机 下 ， 我 们 可 以 把 任意 像素 反 投影 到 三 维 空间 ， 然 后 投影 到 
下 一 幅 图 像 中 。 如 果 在 单 目 相机 中 ， 这 件 事情 要 更 为 困难 ， 因 为 我 们 还 
须 考虑 由 P — 的 深度 带 来 的 不 确定 性 。 详 细 的 深度 估计 放 到 第 13 讲 中 讨 
论 。 现 在 我 们 先 来 考虑 简单 的 情况 ， 即 忆 深 度 已 知 的 情况 。 

根据 P 的 来 源 ， 我 们 可 以 把 直接 法 进行 分 类 : 

LP 来 自 于 稀 玻 关 键 点 ， 我 们 称 之 为 稀疏 直接 法 。 通 常 我 们 使 用 数 
百 个 至 上 千 个 关键 点 ， 并 且 像 L-K 光 流 那样 ， 假 设 它 周 围 像素 也 是 不 变 
的 。 这 种 稀疏 直接 法 不 必 计 算 描述 子 ， 并 且 只 使 用 数 百 个 像素 ， 因 此 束 
度 最 快 ， 但 只 能 计算 稀疏 的 重 构 。 


2P 来 白 部 分 人 像素。 我 们 看 到 式 (8.16) 中 ， 如 果 像 素 梯 上 度 为 零 ， 整 项 
雅 可 比 官 阵 束 为 零 ， 不 会 对 计算 运动 增 量 有 任何 页 献 。 因 此 ， 可 以 考虑 
REA TA RR BERE Rio SAARED. SRIF Bel 
(Semi-Dense) HY fav, nuJbEAXmMJ—-^ 2E S. 


3.P OANA, MAME ARIK Hi ER TA] m Ee VE RNAAR 
(一 般 几 十 万 至 几 百 万 个 ) ， 因 此 多 数 不 能 在 现 有 的 CPU 上 实时 计算 ， 
逢 要 GPU 的 加 速 。 但 是 ， 如 前 面 所 讨论 的 ， 像 系 柳 度 不 明显 的 点 ， 在 运 
动 估计 中 不 会 有 太 大 页 献 ， 在 香 构 时 也 会 难以 估计 位 置 。 

可 以 看 到 ， 从 稀 玩 a 到 向 密 草 构 ， 剖 可 以 用 和 接 法 来 计算 。 它 们 的 计 
算 量 是 逐渐 增长 的 。 稀 玻 方 法 可 以 快速 地 求解 相机 位 姿 ， 而 笛 宅 方法 可 
以 建立 完整 地 图 。 有 只 体 使 用 哪 种 方法 ， 需 要 视 机 项 人 的 应 用 环境 而 定 。 
特别 地 ， 在 低 症 的 计算 平台 上 ， 稀 距 二 接 法 可 以 做 到 非常 快速 的 效 末 ， 
适用 于 实时 性 较 高 且 计 算 资 源 有 限 的 场合 5 。 


8.5 实践 ， RGB-D 的 直接 法 


8.5.1 Wi HIYA 


现在 ， 我 们 来 演示 如 何 使 用 稀疏 的 直接 法 。 由 于 本 书 不 涉及 GPU 编 
程 ， 稠 密 的 直接 法 就 省 略 掉 了 。 同 时 ， 为 了 保持 程序 简单 ， 我 们 使 用 
RGB-D 数 据 而 非 单 目 数 据 ， 这 样 可 以 省 略 抒 单 目的 次 度 恢复 部 分 。 基 于 
特征 点 的 深度 恢复 已 经 在 上 一 讲 介绍 过 ， 而 基于 块 匹 配 的 深度 恢复 将 在 
后 面 介 绍 。 上 所 以 本 贡 我 们 来 考 碟 RGB-D 上 的 稀 踊 直接 法 VO。 


由 于 求解 直接 法 最 后 等 价 于 求解 一 个 优化 问题 ， 因 此 可 以 使 用 g2o 
或 Ceres 这 些 优化 库 来 帮助 求解 。 本 节 以 g20 为 例 设 计 实 验 ， 而 Ceres 部 分 
则 留 作 习题 。 在 使 用 g2o 之 前 ， 需 要 把 直接 法 抽象 成 一 个 图 优化 问题 。 
显然 ， 和 直接 法 是 由 以 下 顶点 和 边 组 成 的 : 


1. 优 化 变量 为 一 个 相机 位 姿 ， 因 此 需要 一 个 位 姿 项 点 。 由 于 我 们 在 
推导 中 使 用 了 至 代数 ， 故 程序 中 使 用 至 代数 表达 的 SE(3) 位 姿 项 扣 。 与 
上 一 讲 一 样 ， 我 们 将 使 用 “VertexSE3Expmap”* 作 为 相机 位 姿 。 

2. 误 天 项 为 单个 像素 的 光 度 误 和 大 。 由 于 整个 优化 过 程 中 [，(pP , tri 
不 变 ， 我 们 可 以 把 它 当 成 一 个 固定 的 预 设 值 ， 然 后 调整 相机 位 姿 ， 使 I ， 
(p, ) 接 近 这 个 值 。 于 是 ， 这 种 边 只 连接 一 个 项 点， 为 一 元 边 。 由 于 g20 
中 本 喘 没 有 计算 光度 误 生 的 边 ， 我 们 需要 目 己 定义 一 种 新 的 边 。 

在 上 述 的 建 模 中 ， 和 直接 法 图 优化 问题 是 由 一 个 相机 位 姿 项 点 与 许多 
条 一 元 边 组 成 的 。 如 果 使 用 和 确 玖 的 直接 法 ， 那 我 们 大 约会 有 几 百 全 几 干 
条 这 样 的 边 ; 稠密 直接 法 则 会 有 几 十 万 条 边 。 优 化 问题 对 应 的 线性 方程 
是 计算 从 代数 增 量 ， 本 刁 规 模 不 大 (6x 6) ， 所 以 主要 的 计算 时 间 会 花 
如 在 每 条 边 的 误差 与 雅 可 比 窍 阵 的 计算 上 。 下 面 的 实验 中 ， 我 们 移 来 定 
义 一 种 用 于 直接 法 位 姿 佑 计 的 边 ， 然 后 ， 使 用 该 边 构 建 图 优化 问题 并 求 
AE o 


8.5.2 ”定义 直接 法 的 边 


首先 定义 计算 光度 误 兰 的 边 。 按 照 前 面 的 推导 ， 还 需要 给 出 它 的 雅 
Ay EB EE: 
由 slambook/ch8/directMethod/direct_sparse.cpp C Fr Ex) 


// project a 3d point into an image plane, the error is photometric error 
// an unary edge with one vertex SE3Ezpmap (the pose of camera) 
Class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


EdgeSE3ProjectDirect() {} 

EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, 
float cy, cv::Mat* image ) : x world, ( point ), fx ( fx ), fy. ( fy ), cx_ ( 
cx ), cy. ( cy ), image. ( image ) 


{} 


virtual void computeError () 


{ 
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( vertices 
[0] ); 
Eigen::Vector3d x local = v-»estimate().map ( x world, ); 
float x » x local[0]*fx /x local[2] * cx.; 
float y = x local[i1]*fy /x local[2] + cy_; 
// check x,y is in the image 
if ( x-4«0 || ( x*4 ) >image_->cols || ( y-4 ) «0 || ( y*4 ) >image_->rows 
) 
1 
.error (0,0) = 0.0; 
this->setLevel ( 1 ); 
) 
else 
1 
.error ( 0,0 ) = getPixelValue ( x,y ) - measurement; 
) 
) 


// plus in manifold 
virtual void linearizeOplus( ) 
i 

if ( level() == 1) 

1 


_jacobianOplusXi = Eigen::Matrix«double, 1, 6>::Zero(); 
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return; 


} 


VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> (  vertices[0] ); 


Eigen::Vector3d xyz_trans 


book 
double x = xyz trans[0]; 
double y = xyz trans[1]; 


vtx-»estimate().map ( x world, ); // q in 


double invz - 1.0/xyz trans[2]; 


double invz 2 = invz*invz 


float u 


. 
3 


x*fx *invz + cx: 


float v = y*fy *invz + cy. ; 


// jacobian from se3 to u,v 


// NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is 


so(3) and \epsilon the translation 


Eigen: :Matrix<double, 2, 
jacobian_uv_ksai ( 0,0 ) 
jacobian_uv_ksai ( 0,1 ) 
jacobian_uv_ksai ( 0,2 ) 
jacobian_uv_ksai ( 0,3 ) 
jacobian_uv_ksai ( 0,4 ) 
jacobian_uv_ksai ( 0,5 ) 
jacobian_uv_ksai ( 1,0 ) 
jacobian_uv_ksai ( 1,1 ) 
jacobian_uv_ksai ( 1,2 ) 
jacobian_uv_ksai ( 1,3 ) 
jacobian_uv_ksai ( 1,4 ) 
jacobian_uv_ksai ( 1,5 ) 


Eigen: :Matrix<double, 1, 


jacobian, pixel uv ( 0,0 ) 
2 J £21 
jacobian, pixel uv ( 0,1 ) 
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6» 


2» 


jacobian uv ksai; 


一 x*y*invz 2 *fx_; 

( 1+ ( x*x*invz 2 ) ) *fx_; 
- y*invz *fx ; 

invz *f£x_; 

0; 


-x*invz 2 *fx_; 


- ( 1*y*y*invz 2 ) *fy_; 
x*y*invz 2 *fy ; 

x*invz *fy ; 

0; 

invz *fy ; 


-y*invz 2 *fy_; 


jacobian pixel uv; 


( getPixelValue ( u*i1,v )-getPixelValue ( u-1,v 


( getPixelValue ( u,v*1 )-getPixelValue ( u,v-1 


_jacobianOplusXi = jacobian pixel uv*jacobian uv ksai; 


// dummy read and write functions because we don't care... 


virtual bool read ( std::istream& in ) {} 


78 virtual bool write ( std::ostream& out ) const {} 


80 | protected: 


81 // get a gray scale value from reference image (bilinear interpolated) 
82 inline float getPixelValue ( float x, float y ) 

83 1 

84 uchar* data = & image -»data[ int ( y ) * image -^5step + int ( x ) ]; 
85 float xx s xr = floor ( x ); 

86 float yy = y = floor (y j; 

87 return float ( 

88 C 1-xx ) € ( T-yy J * datald] + 

89 xx* ( 1-yy ) * data[1] + 

90 ( 1-xx ) *yy*data[ image -»5step ] + 

91 xx*yy*data [image -»step*1] 

92 i 

93 } 

94 | public: 

95 Eigen::Vector3d x_world_; // 3D point in world frame 

96 float cx 7-0, cy -0, fx -0, fy -0; // Camera intrinsics 

97 cv::Mat* image, -nullptr; // reference image 

9$ | 


我 们 的 边 继 承 自 g2o::BaseUnaryEdge。 在 继承 时 ， 需 要 在 模板 参数 
里 填 入 测量 值 的 维度 、 美 型 ， 以 及 连接 此 边 的 项 点 ， 同 时， 我 们 把 空间 
AP  、 相 机 内 参 和 图 像 存 储 在 该 边 的 成 员 变 量 中 。 为 了 让 g2o 优 化 该 边 
DVN RA, RIER SU MEER: 用 computeError0O 计 算 误 天 
(B, HjlinearizeOplusOiF $E AY EAER. AP ELE], ix ELSE RJ EGRE 
计算 与 式 (8.16) 是 一 致 的 。 注 意 我 们 在 程序 中 的 误 舌 计算 里 使 用 了 T，,(C ， 
MH ,(p,) Wes, ALN Hu WBE, Hut REUS IR 
到 他 代数 的 梯度 即 可 。 

在 程序 中 ， 相 机 位 姿 是 用 浮 点 数 表 示 的 ， 投 影 到 像素 坐标 也 是 浮 点 
形式 。 为 了 更 精细 地 计算 像素 完 度 ， 我 们 要 对 图 像 进行 插值 。 我 们 这 里 
采用 了 简单 的 双 线 性 插值 ， 也 可 以 使 用 更 复杂 的 插值 方式 ， 但 计算 代价 
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8.5.3 ”使 用 直接 法 估计 相机 运动 


定义 了 g2o 边 后 ， 我 们 将 节点 和 边 组 合成 图 ， 就 可 以 调用 g2o 进 行 优 
化 了 。 实 现代 人 码 位 于 slambook/ch8/directMethod/direct_sparse.cpp 中 ， 请 
读者 阅读 该 部 分 代 公 并 编 详 。 

在 这 个 实验 中 ， 我 们 读 取 数 据 集 的 RGB-D 图 像 序 列 。 以 第 一 幅 图 像 
NES wl, AGA BRK AeA RNA. FEBS, OSS 
图 像 提取 FAST 关 键 点 〈 不 需要 摘 述 子 ) ， 并 使 用 直接 法 估计 这 些 天 键 
点 在 第 二 幅 图 像 中 的 位 置 ， 以 及 第 二 幅 图 像 的 相机 位 姿 。 这 束 构 成 了 一 
种 简单 的 稀 玖 直接 法 。 最 后 ， 我 们 面 出 这 些 关 键 点 在 第 二 幅 图 像 中 的 投 
SU 。 


/一 P. 
执行 如 下 命 令 : 
1 | build/direct sparse -/dataset/rgbd dataset freiburgi desk 


程序 会 在 作 图 之 后 暂停 ， 你 可 以 看 到 特征 点 的 位 置 关 系 ， 终 凯 也 会 
46] LIA TUR ZE HI] P BELA o 

如 图 8-4 所 示 ， 我 们 看 到 在 两 幅 图 像 相 差不多 时 ， 下 接 法 会 调整 相 
机 的 位 姿 ， 使 得 大 部 分 像素 都 能 够 正确 跟踪 。 但 是 ， 在 稍 长 一 点 的 时 间 
内 ， 比 如 说 0 一 9 下 之 间 的 对 比 ， 我 们 友 现 由 于 相机 位 姿 估 计 不 准确 ， 特 
征 点 出 现 了 明显 的 偏 移 现象 。 我 们 会 在 本 讲 末 尾 对 其 进行 分 析 。 
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18-4 MRAR. Æ: RAMEKA BER. £1: 参考 帧 与 后 1 一 9 帧 对 比 《〈“ 选 取 
部 分 关键 点 ) o 


8.5.4 "4A 2 ARE 
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提取 梯度 较 明 显 的 像 系 ， 然 后 用 和 耳 接 法 ， 以 这 些 像 系 为 图 优化 边 来 估计 
相机 运动 。 对 先前 的 程序 做 如 下 修改 : 


四 slambook/ch8/direct. semidense.cpp 





1 |// select the pizels with high gradiants 


2 |for ( int x-10; x<gray.cols-10; x++ ) 


3 for ( int y=10; y<gray.rows-10; yt+ ) 

4 ji 

5 Eigen::Vector2d delta ( 

6 gray.ptr<uchar>(y) [x+1] = gray.ptr<uchar>(y) [x-1], 
7 gray.ptr<uchar>(y+1) [x] - gray.ptr<uchar>(y-1) [x] 


8 J; 


9 if ( delta.norm() « 50 ) 
10 continue; 


T ushort d = depth.ptr<ushort> (y) [x]; 


12 if ( d==0 ) 

13 continue; 

14 Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth scale 
); 

15 float grayscale = float ( gray.ptr<uchar> (y) [x] ); 

16 measurements.push_back ( Measurement ( p3d, grayscale ) ); 

17 } 





xx Ae MR Ee. RAIER Pe RE xx EE, SA HA 
显 梯度 的 像 系 。 于 是 在 图 优化 中 会 增加 许多 的 边 。 这 些 边 都 会 参与 估计 
相机 位 姿 的 优化 问题 ， 利 用 大 量 的 像 际 而 不 单单 是 稀 下 的 特征 点 。 由 于 
我 们 并 没有 使 用 所 有 的 像 尿 ， 所 以 这 种 方式 又 称 为 半 移 密 方法 〈 Semi- 
dense) ”。 我 们 把 参与 估计 的 像 系 取出 来 并 在 图 像 中 显示 出 来 ， 如 图 8-5 
所 示 。 
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图 8-5” 半 稠密 直接 法 的 实验 。 参 考 帧 与 第 2,5,8 帧 的 对 比 ， 绿 色 部 分 标 出 了 参与 优化 的 像 
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如 各 该 者 杀 目 做 了 实验 ， 网 可 以 看 到 参与 估计 的 像 系 像 是 固定 在 圣 
间 中 一 样 ”。 当 相机 旋转 时 ， 它 们 的 位 置 似乎 没有 及 生变 化 。 这 代表 了 
我 们 佑 计 的 相机 运动 是 正确 的 。 同 时 ， 你 可 以 检查 使 用 的 像素 数量 与 优 
化 时 间 的 关系 。 蛙 然 ， 当 像素 增多 时 ， 优 化 会 更 加 肌 时 ， 所 以 为 了 实时 
性 ， 需 要 孝 层 使 用 较 好 的 像素 点， 或 者 降低 图 像 的 分 辨 率 。 不 过 对 于 汇 
示 实 验 来 说 ， 笔 者 认为 这 样 已 经 能 够 理解 直接 法 的 意义 了 。 


8.5.5 ”直接 法 的 讨论 


相 比 于 特征 点 法 ， 直 接 法 完全 依靠 优化 来 求解 相机 位 姿 。 从 去 
(8.16) 中 可 以 看 到 ， 像 系 梯 度 引 叶 看 优化 的 方 回 。 如 来 想 要 得 到 正确 的 
优化 结 霖 ， 束 必须 你 证 大 部 分 像 系 构 度 能 够 把 优化 引导 到 正确 的 方 问 


pe ABE? 我 们 不 妨 设 号 处 地 地 扮演 一 下 优化 算法 。 假 设 对 
于 参考 图 像 ， 我 们 测量 a 到 一 个 灰 度 值 为 229 的 像 系 。 并 且 ， 由 于 我 们 知 
思 它 的 深度 ， 可 以 推断 出 空间 点 P 的 位 置 〈“ 图 8-6 所 示 在 六 中 测量 到 的 砍 


度 ) 。 


更新 一 次 后 
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此 时 我 们 又 得 到 了 一 幅 新 的 图 像 ， 需 要 估计 和 它 的 相机 位 姿 。 这 个 位 
次 是 由 一 个 初 值 不 断 地 优化 迭代 得 到 的 。 假 设 我 们 的 初 值 比较 差 ， 在 这 
OMAR, THAP SuRBUEGRIKIRTHOE126. FÆ, IMRAN 
2:74229- 126=103。 为 了 减 小 这 个 误差 ， 我 们 希望 微调 相机 的 位 姿 ， 使 
像素 更 亮 一 些 。 

BA FUEL EAB FE Ge a RA SE EE? 这 束 需 要 用 到 局 部 的 像素 梯 
E. BONER PAH, Yu 轴 往 前 走 一 步 ， 该 处 的 灰 度 值 变 成 了 
123， 即 减 去 了 了 3。 同样 地 ， 沿 vy 轴 往 前 走 一 步 ， 灰 度 值 减 了 18， 变 成 
108。 在 这 个 像 系 周围 ， 我 们 看 到 梯度 是 [- 3,- 18]， 为 了 提高 完 度 ， 我 们 
会 建议 优化 算法 微调 相机 ， 使 P 的 像 往 左上 方 移动 。 在 这 个 过 程 中 ， 我 
们 用 像素 的 局 部 梯度 近似 了 它 附 近 的 灰 度 分 布 ， 不 过 请 注意 ， 真 实 图 像 
并 不 是 光滑 的 ， 所 以 这 个 梯度 在 远 处 就 不 成 并 了 。 

但 是 ， 优 化 算法 不 能 只 昕 这 个 像 系 的 一 面 之 词 ， 还 需要 听取 其 他 像 
素 的 建议 中 。 综 合 昕 取 了 许多 像素 的 意见 之 后 ， 优 化 算法 选择 了 一 个 和 
我 们 建议 的 方 辐 偏离 不 远 的 地 方 ， 计 算出 一 个 更 新 量 exp(E*”)。 加 上 更 
ele, BKM , 移动 到 了 2 ， 像 素 的 投影 位 置 也 变 到 了 一 个 更 亮 的 地 
方 。 我 们 看 到 ， 通 过 这 次 更 新 ， 误 差 变 小 了 。 在 理想 情况 下 ， 我 们 期 
FREAD Phe, wR Ce. 

但 是 实际 是 不 是 这 样 呢 ? BOT AE BN REY 7 AE, LAE 
走 到 一 个 最 优 值 ? 注意 到 ， 直 接 法 的 梯度 是 直接 由 图 像 梯 度 确 定 的 ， 因 
此 我 们 必须 保证 沿 着 图 像 梯度 走时 ， 灰 度 误 兰 会 不 断 下 降 . vu. A 
像 通 党 是 一 个 很 强烈 的 非 凸 函数 ， 如 图 8-7 所 示 。 实 际 当中 ， 如 果 我 们 
WERE, (RAE TARAS SSE SE Cake) 洛 进 一 个 
局 部 极 小 值 中 ， 无 法 继续 优化 。 只 有 当 相 机 运动 很 小 ， 图 像 中 的 梯度 不 
会 有 很 强 的 非 凸 性 时 ， 直 接 法 才能 成 立 。 





图 8-7 一 张 图 像 的 三 维 化 显示 。 从 图 像 中 的 一 个 扣 运 动 到 为 一 个 后 的 路 径 不 见得 是 “ 笔 耳 
的 下 坡 路 *， 而 需要 经 第 “ 翻 山 越 岭 *。 这 体现 了 图 像 本 里 的 非 凸 性 。 


EEF, BANAT SEPBANEH, FARAH EH KE 
直接 相 减 得 到 的 。 然 而 ， 单 个 像 北 没有 什么 区 分 性 ， 周 围 很 可 能 有 好 多 
像素 和 它 的 之 上 度 玫 不 多 。 上 所以， 我 们 有 时 会 使 用 小 的 图 像 块 Catch) ， 
并 且 使 用 更 复杂 的 乔 异 度量 方式 ， 例 如 归 一 化 相关 性 (Normalized 
Cross Correlation, NCC) 等 〈 见 第 13 讲 ) 。 而 例 程 为 了 简单 起 见 ， 使 用 
了 误 芭 的 平方 和 和， 以 保持 与 推导 的 一 致 性 。 


8.5.6 ”直接 法 优 缺 点 总 结 


最 后 ， 我 们 总 结 一 下 直接 法 的 优 缺 点 。 大 体 上 ， 它 的 优点 如 下 : 
“可 以 省 去 计算 特征 点 、 描 述 子 的 时 间 。 


“只 要 求 有 像 系 柳 度 即 可 ， 不 需要 特征 点 。 因 此 ， 和 下 接 法 可 以 在 特 
征 缺 失 的 场合 下 使 用 。 比 较 极 器 的 例子 是 只 有 渐变 的 一 幅 图 像 。 它 可 能 
无 法 提取 角 扣 类 特征 ， 但 可 以 用 年 接 法 信 计 它 的 运动 。 


“可 以 构建 半 笛 密 旋 至 稠密 的 地 图 ， 这 征 特 征 点 法 无 法 做 到 的 。 
为 一 方面 ， 它 的 缺 扣 也 很 明显 : 
“ 非 凸 性 ”。 征 接 法 完全 依 徘 标 度 搜索 ， 降 低 目 标 函 数 来 计算 相机 位 


姿 。 其 目标 函数 中 需要 取 像 素 点 的 灰 度 值 ， 而 图 像 是 强烈 非 辐 的 函数 。 
这 使 得 优化 算法 容易 进入 极 小 ， 只 在 运动 很 小 时 直接 法 才能 成 功 。 

“单个 像素 没有 区 分 度 。 和 它 像 的 实在 太 多 了 ! 于 是 我 们 要 么 计算 
图 像 块 ， 要 么 计算 复杂 的 相关 性 。 由 于 每 个 像素 对 改变 相机 运动 的 “ 意 
见 ? 不 一 致 ， 只 能 少数 服从 多 数 ， 以 数量 代 蔡 质量 。 

* 灰 上 度 值 不 变 是 很 强 的 假设 。 如 有 果 相 机 是 目 动 曝光 的 ， 当 它 调 整 蚂 
光 参 数 时 ， 会 使 得 图 像 整 体 变 亮 或 变 上 暗 。 光 照 变化 时 亦 会 出 现 这 种 情 
况 。 特 征 点 法 对 光照 具有 一 定 的 容忍 性 ， 而 直接 法 由 于 计算 灰 度 间 的 差 
异 ， 整 体 灰 度 变 化 会 破坏 灰 度 不 变 假 议 ， 使 算法 失败 。 针 对 这 一 点 ， 目 
六 的 直接 法 开始 使 用 更 细致 的 光度 模型 标定 相机 ， 以 便 在 明 光 时 间 变 化 
时 也 能 工作 。 

习题 

1. 除 了 LK 光 流 之 外 ， 还 有 哪些 光 流 方法 ? 它们 各 有 什么 特点 ? 

2. 在 本 节 程 序 的 求 图 像 梯 度 过 程 中 ， 我 们 简单 地 求 了 u +1 和 uw- 1 的 灰 
BIZ ZSERUA2. fEZgju 方 回 上 的 构 度 值 。 这 种 做 法 有 什么 缺点 ?提示 : 
对 于 距离 较 近 的 特征 ， 变 化 应 该 较 快 ， 而 距离 较 远 的 特征 在 图 像 中 变化 
较 慢 ， 求 梯度 时 能 人 耕 利 用 此 信息 ? 

3. 在 黎 蚊 直接 法 中 ， 假 设 时 个 像 系 周围 小 块 的 光度 也 不 变 ， 是 否 可 
以 提高 算法 健壮 性 ? 请 编程 实现 。 

4.* fii FH] CeresS: E RGB-D E A tit Bak AE He 2 ECRIRE 

5. 相 比 于 RGB-D 的 直接 法 ， 单 目 直 接 法 往往 更 加 复杂 。 除 了 匹配 未 
知之 外 ， 像 素 的 距离 也 是 竺 估计 的 ， 我 们 需要 在 优化 时 把 像素 深度 也 作 
为 优化 变量 。 阅 读 文献 [59,57]， 你 能 理解 它 的 原理 吗 ? 如果 不 能 ， 请 在 
第 13 讲 后 再 回来 阅读 。 

6. 由 于 图 像 的 非 凸 性 ， 直 接 法 目前 还 只 能 用 于 短 距 离 、 非 自动 曝光 
的 相机 。 你 能 否 提 出 增强 直接 法 健壮 性 的 方案 ?阅读 文献 [58,60] 可 能 会 


给 你 一 些 灵 感 。 





[1] YLhttp://vision.in.tum.de/data/datasets/rgbd-dataset/download . 
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变化 。 它 不 会 影响 公式 的 推导 。 


[3] 这 可 能 是 一 种 不 严谨 的 拟人 化 说 法 ， 不 过 有 助 于 理解 。 
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主要 目标 

1. 实 际 设 计 一 个 视 党 里 程 计 前 端 。 

2. 理 解 SLAM 软 件 框架 是 如 何 搭 建 的 。 

3. 理 解 在 前 端 设计 中 容易 出 现 的 问题 ， 以 及 修补 的 方式 。 

本 讲 是 全 书 比较 少见 的 完全 由 实践 部 分 组 成 的 一 讲 。 我 们 将 使 用 前 
两 讲学 到 的 知识 ， 实 际 书写 一 个 视觉 里 程 计 程序 。 你 会 管理 局 部 的 机 器 
人 轨迹 与 路 标点 ， 并 体验 一 下 一 个 软件 框架 是 如 何 组 成 的 。 在 操作 过 程 
中 ， 我 们 会 遇 到 许多 问题 : 相机 运动 过 快 、 图 像 模 糊 、 误 匹配 .………. 都 会 
使 算法 失效 。 要 让 程序 稳定 运行 ， 我 们 需要 处 理 以 上 的 种 种 情况 ， 这 将 
带 来 许多 工程 实现 方面 的 、 有 益 的 讨论 。 


9.1 搭建 VO 框 架 


AH b A ARE ER, FR ANI He is SE FAHY Ere 
在 笔者 深 爱 的 《我 的 世界 》 游 戏 中 ， 玩 家 拥有 的 只 是 一 些 色 彩 、 纹 
理 不 同 的 方 芯 。 其 性 质 极其 简单 ， 而 玩家 所 要 做 的 只 是 把 这 些 方 英 放 在 
宝地 上 而 已 。 理 解 一 个 方块 至 为 商 单 ， 但 实际 合 起 它们 时 ， 初 学 者 往往 
只 能 搭建 简单 的 火柴 盒 房 屋 ， 而 有 经 验 、 有 创造 力 的 玩家 则 可 用 这 些 简 
单 的 方块 建造 民 牛 、 园 林 、 楼 人 台 孚 栅 ， 力 全 城市 《多 9-1) H., 





图 9-1 从 简单 的 事物 出 及， 逐渐 搭建 越 来 越 复 杂 但 越 来 越 优 秀 的 作品 。 


在 SLAM 中 ， 我 们 认为 工程 实现 和 理解 算法 原理 应 该 至 少 是 同等 重 
要 的 ， 甚 至 更 应 强调 如 何 书写 实际 可 用 的 程序 。 算 法 的 原理 ， 就 像 一 个 
个 方块 一 样 ， 我 们 可 以 清楚 明确 地 讨论 它们 的 原理 和 性 质 ， 但 仅仅 理解 
了 一 个 个 方块 并 不 能 使 你 建造 真正 的 建筑 : 它们 需要 大 量 的 尝试 、 时 间 
和 经 验 ， 我 们 豆 励 读者 级 更 为 实际 的 方 同 努力 一 当然 这 往往 是 十 分 复 
ARH. HAZE (ERATE SL) EIE, (REE Se EE. to. ali 
War, Fe REE, JL A RE hae, uerum MRT BERN 
块 的 性 质 那 样 简 单 。 


SLAM 的 具体 实现 亦 是 如 此 ， 一 个 实用 的 程序 会 有 很 多 的 工程 设计 


MFI (Trick) ， 还 需要 讨论 每 一 步 出 现 问题 乙 后 该 如 何 处 理 。 原 则 上 
讲 ， 每 个 人 实现 的 SLAM 都 会 有 所 不 同 ， 多 数 时 候 我 们 并 不 能 说 哪 种 实 
现 方式 就 一 定 是 最 好 的 。 但 是 ， 我 们 通 间 会 遇 到 一 些 共 同 的 问题 : “ 怎 
么 管理 地 图 点 关 如 何 处 理 误 匹配 ”如 何 选择 天 键 帆 ”， 等 等 。 我 们 希望 
谈 者 能 对 这 些 可 能 出 现 的 问题 产生 一 些 特 观 的 感觉 一 一 我 们 认为 这 种 感 
党 是 非 浓重 要 的 。 

所 以 ， 出 于 对 实践 的 重视 ， 本 章 我 们 将 币 领 读者 领略 一 下 搭建 
SLAM 框 染 的 过 程 。 束 像 建筑 那样 ， 我 们 要 讨论 柱 间 距 、 门 面 沉 蜗 比 等 
玉 雁 但 重要 的 问题 。SLAM 工 程 是 复杂 的 。 即 使 我 们 只 保留 核心 的 部 
分 ， 也 会 占用 大量 的 篇 幅 ， 使 本 书 变 得 过 于 楷 几 。 不 过 ， 请 注意 ， 尽 管 
完成 之 后 的 工程 是 复杂 的 ， 但 是 中 间 的 “由 简 到 老 ” 的 过 程 ， 古 值得 详细 
讨论 、 有 学 习 价 值 的 。 所 以 ， 我 们 要 从 简单 的 数据 结构 出 及， 先 来 做 一 
个 徐 单 的 视 党 里 程 计 ， 上 再 悍 悍 地 把 一 些 额外 的 功能 加 进来 。 换 言 之 ， 我 
们 要 把 从 简单 到 复杂 ”的 过 程 展现 给 读者 看 ， 这 样 你 才 会 明日 一 个 库 是 
如 何 像 雪 人 那样 慢 慢 堆 起 来 的 。 

本 讲 的 代码 放 在 slambook/project 中 。 由 于 随 看 开 肥 过 程 不 断 前 进 ， 
我 们 会 对 工程 做 一 些 删 改 ， 因 此 它 的 内 容 也 会 发 生变 化 。 所 以 我 们 会 把 
中 间 的 代码 也 保留 在 目录 中 ， 以 版 本 号 命名， 以 便 读 者 随时 但 看 、 模 
仿 。 


9.1.1 确定 程序 框 染 


根据 前 两 讲 的 内 容 ， 我 们 知道 视 党 里 程 计 分 单 目 、 双 目 、RGB-D 三 
大 类 。 单 目 视 觉 相 对 复杂 ， 而 RGB-D 最 为 简单 ， 没 有 初始 化 ， 也 没有 尺 
上 度 问 题 。 本 痢 由 人 简 入 索 的 指导 思想 ， 我 们 先 从 RGB-D 做 起 。 为 了 方便 读 
者 做 实验 ， 我 们 将 使 用 数据 集 而 非 实 际 的 RGB-D 相 机 《因为 不 能 保证 旋 
者 人 手 一 台 RGB-D 相 机 ) 。 


首先 ， 我 们 来 了 解 一 下 Linux 程 序 的 组 织 方式 。 在 编写 一 个 小 规模 
的 库 时 ， 我 们 通 间 会 建立 一 些 文件 严 ， 把 涯 代码 、 头 文件 、 文 要 、 测 试 
数据 、 配 置 文件 、 日 志 等 分 类 存放 ， 这 样 会 显得 很 有 和 条理。 如 果 一 个 库 
内 容 很 多 ， 我 们 还 会 把 代码 分 解 成 各 个 独立 的 小 模块 ， 以 便 测 试 。 读 者 
可 以 参照 OpenCV 或 g2o 的 组 织 方式 ， 看 看 一 个 大 中 型 库 是 如 何 组 织 的 。 





例如 ，OpenCV 有 core、imgproc、features2d 等 模块 ， 每 个 模块 分 别 负 责 
不 同 的 任务 。g20 则 有 core、solvers、types 等 右 干 模块 。 不 过 在 小 型 程 订 
里 ， 我 们 也 可 以 把 所 有 的 东西 帮 ' 在 一 起 ， 称 为 SLAM 库 。 


现在 我 们 要 写 的 SLAM 库 是 一 个 小 型 库 ， 目 标 是 帮 读 者 将 本 书 用 到 
的 各 种 算法 融会 贯通 ， 书 写 自 己 的 SLAM 程 序 。 挑 选 一 个 工程 目录 ， 在 
其 下 面 建立 如 下 文件 夹 来 组 织 代码 文件 : 


1.bin 用 来 存放 可 执行 的 二 进 制 文件 。 


2.include/myslam 存 放 SLAM 模 坎 的 头 文 件 ， 主 要 是 .hn 文件 。 这 种 做 
法 的 理由 是 ， 当 把 包含 目录 设 到 include， 引 用 目 己 的 头 文 件 时 ， 需 要 写 
include"myslam/xxx.h"， 这 样 不 容易 和 列 的 库 混 消 。 

3.src 和 存放 着 代码 文件 ， 主 要 是 .cpp 文 件 。 

4.test 存 放 测 试用 的 文件 ， 也 十.cpp 文 件 。 

5.]lib 和 存放 编 详 好 的 库 文 件 。 

6.con fi gf WAC NF- 

7.cmake_modules 第 三 方 库 的 cmake 文 件 ， 在 使 用 g2o 之 类 的 库 时 会 
Hale. 

EL EWER HRA, uo-2ppyzk. THEE T ZA E HEA 
零散 敬 地 放羊 的 main.cpp， 这 种 做 法 显得 更 有 条 理 。 接 下 来 ， 我 们 会 在 
这 些 目录 里 不 断 地 添加 新 文件 ， 逐 潮 形 成 一 个 完整 的 程序 。 


€ > ”从 主 文件 夹 sources slambook project 0.1 
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图 9-2 工程 项 目的 目录 。 


9.1.2 人 确定 基本 数据 结构 


为 了 让 程序 跑 起 来 ， 我 们 要 设计 好 数据 单元 ， 以 及 程序 处 理 的 流 
程 。 这 好 比 构成 房屋 的 一 个 个 的 柱子 和 砖 块 。 那 么 ， 在 一 个 SLAM 程 序 
中 ， 有 哪些 结构 是 最 基本 的 呢 ? 我 们 抽象 出 以 下 基本 概念 : 


Lt: 帆 是 相机 采集 到 的 图 像 单位 。 它 主要 包含 一 个 图 像 (RGB-D 
情形 下 是 一 对 图 像 ) 。 上 此外， 还 有 特征 点 、 位 姿 、 内 参 等 信息 。 在 视觉 
SLAM 中 我 们 会 谈论 关键 帧 (Key-frame) 。 由 于 相机 采集 的 数据 很 多 ， 
存储 所 有 的 数据 显然 是 不 现实 的 。 人 否则 ， 如 果 相 机 放 在 条 上 不 动 ， 程 序 
的 内 存 占 用 也 会 越 来 越 高 百人 至 无 法 接受 。 通 第 的 做 法 是 把 东 些 我 们 认为 
更 重要 的 帧 保存 起 来 ， 并 认为 相机 轨迹 可 以 用 这 些 关 键 帆 来 搬 述 。 关 键 
由 如 何 选 择 是 一 个 很 大 的 问题 ， 而 且 基 于 工程 经 验 ， 很 少 有 理论 上 的 指 
导 。 在 本 书 中 我 们 会 使 用 一 个 关键 帧 选择 方 法 ， 但 读者 亦 可 考虑 目 己 提 
出 新 的 方式 。 


2 .路 标 : 路 标点 即 图 像 中 的 特征 点 。 在 相机 运动 后 ， 我 们 还 能 估计 
它们 的 3D 位 置 。 通 常 ， 会 把 路 标点 放 在 一 个 地 图 ”当中 ， 并 将 新 来 的 帧 


与 地 图 中 的 路 标点 进行 匹配 ， 佑 计 相 机 位 姿 。 


怖 的 位 姿 与 路 标的 位 置 估计 相当 于 一 个 局 部 的 SLAM 问 题 。 除 此 之 
外 ， 我 们 还 需要 一 些 工具 ， 让 程序 写 起 来 更 流畅 。 例 如 


LEAX : 在 写 程序 过 程 中 你 会 经 第 过 到 各 种 各 样 的 参数 ， 比 
如 ， 相 机 的 内 参 、 特 征 点 的 数量 、 匹 配 时 选择 的 比例 ， 等 等 。 你 可 以 把 
这 些 数 写 在 程序 中 ， 但 那个 是 一 个 好 习惯 。 你 会 经 党 修改 这 些 参 数 ， 但 
每 次 修改 后 都 要 重新 编译 一 过 程序 。 当 其 数量 越 来 越 多 时 ， 修 改 就 变 得 
越 来 越 困 难 。 所 以 ， 更 好 的 方式 是 在 外 部 定义 一 个 配置 文件 ， 程 序 运 行 
时 谈 取 该 配置 文件 中 的 参数 值 。 这 样 ， 每 次 只 要 修改 配置 文件 内 容 融 行 
了 了， 不必 对 程序 本 时 做 任何 修改 。 


2. 坐 标 变换 : 你 会 经 沼 需 要 在 坐标 系 则 进行 坐标 变换 ， 例 如 ， 世 界 
坐标 到 相机 坐标 、 相 机 坐标 到 归 一 化 相机 坐标 、 归 一 化 相机 坐标 到 像素 
坐标 ， 等 等 。 定 义 一 个 类 把 这 些 操 作 都 放 在 一 起 将 更 方便 。 

下 面 我 们 束 来 定义 帜 、 足 标 这 几 个 概念 ， 在 C++ 中 都 以 类 来 表示 。 
我 们 尽量 保证 一 个 类 有 单独 的 头 文件 和 源 文 件 ， 避 人 免 把 许多 个 类 放 在 同 
一 个 文件 中 。 然 后 ， 把 函数 声明 放 在 头 文 件 ， 实 现 放 在 源 文 件 中 《除非 
图 数 很 得 ， 也 可 以 写 在 头 文件 中 ) 。 我 们 参照 Google 的 命名 规范 ， 同 时 
考虑 尽量 以 初学 者 也 能 看 懂 的 方式 来 写 程 序 。 由 于 我 们 的 程序 是 偏 问 算 
法 而 非 软件 工程 的 ， 所 以 不 讨论 复杂 的 次 继 承 关 系 、 接 口 、 模 板 等 ， 而 
更 关注 算法 的 正确 实现 ， 以 及 是 个 便于 扩展 。 我 们 会 把 数据 成 员 设 置 
为 公有 有， 尽管 这 在 C++ 软件 设计 中 是 应 该 避免 的 ， 如 果 读 者 愿意 ， 也 可 
DEE TI ARM private protected, FAVS SARA. TERE 
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需要 修改 整个 运行 流程 ， 只 需 调 整 局 部 的 处 理 方式 即 可 。 

现在 ， 让 我 们 开始 写 VO。 我 们 把 这 个 版 本 定 为 0.1 版 ， 表 示 这 是 刚 
开始 的 阶段 。 我 们 一 共 瑟 5 个 类 : Frame 为 帧 ，Camera 为 相机 模型 ， 
MapPoint 为 特征 点 /路 标点 ，Map 管 理 特 征 点 ，Con fi g 提 供 配 置 参 数 。 
它们 的 关系 如 图 9-3 所 示 。 我 们 现在 只 写 它们 的 数据 成 员 和 常用 方法 ， 
而 在 后 面 用 到 更 多 内 容 时 再 行 添 加 。 
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图 9-3 ”基本 类 的 关系 示意 图 。 


Camera 类 最 人 简单， 我 们 先 来 实现 它 。 





9.1.3 Camera 类 


Camera 类 存储 相机 的 内 参 和 外 参 ， 并 完成 相机 坐标 系 、 像 系 坐 标 系 
和 世界 坐标 系 之 间 的 坐标 变换 。 当 然 ， 在 世界 坐标 系 中 你 需要 一 个 相机 
的 《变动 的 ) 外 参 ， 我 们 以 参数 的 形式 传 入 。 


四 slambook/project/0.1/include/myslam/camera.h 


| |#ifndef CAMERA H 
2 |#define CAMERA H 
3 | ftinclude "myslam/common include.h" 
4 |namespace myslam 

$ 
6 |// Pinhole RGB-D camera model 
7 | class Camera 
s [t 
9 | public: 
10 typedef std::shared_ptr<Camera> Ptr; 


T float fx , fy_, cx ., cy_, depth scale, ; // Camera intrinsics 


13 Camera() ; 

14 Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) : 

15 fx fx ), fy. C fy ), ex ( cx ), ey ( cy ), depth scale, ( depth scale ) 
16 {} 

17 

18 // coordinate transform: world, camera, pizel 

19 Vector3d world2camera( const Vector3d& p_w, const SE3&£ T_c_w ); 

20 Vector3d camera2world( const Vector3d& p c, const SE3& T c w ); 

21 Vector2d camera2pixel( const Vector3d& p_c ); 

22 Vector3d pixel2camera( const Vector2d& p p, double depth=1 ) ; 

23 Vector3d pixel2world ( const Vector2d& p p, const SE3& T_c_w, double depth=1 ); 
24 Vector2d world2pixel ( const Vector3d& p_w, const SE3& T c w ); 





27 | #endif // CAMERA H 


说 明 如 下 (由 上 人 往 下 ): 

1. 在 这 个 简单 的 例子 中 ， 我 们 给 出 了 防止 头 文件 重复 引用 的 itndef 安 
定义 。 如 果 没 有 这 个 宏 ， 在 两 处 引用 此 头 文件 时 将 出 现 类 的 重复 定义 。 
所 以 ， 在 每 个 程序 头 文 件 里 都 会 定义 这 样 一 个 宏 。 

2. 我 们 用 命名 空间 namespace myslam 将 关 定 义 包 囊 起 来 〈 因 为 是 我 
们 目 己 写 的 SLAM， 所 以 命名 空间 束 叫 myslam 了) 。 命 名 空间 可 以 防止 
我 们 不 小 心 定 义 出 列 的 库 里 同名 的 函数 ， 也 是 一 种 比较 安全 和 规 苑 的 做 
法 。 由 于 宏 定 义 和 命 名 空间 在 每 个 文件 中 都 会 写 一 人 表 ， 所 以 我 们 只 在 这 


里 稍 加 介绍 ， 后 面 将 略 去 。 

3. 我 们 把 一 些 间 用 的 头 文 件 放 在 common_include.h 文 件 中 ， 这 样 残 
可 以 避免 每 次 书写 很 长 的 一 串 include。 

4. 我 们 把 智能 指针 定义 成 Camera 的 指针 类 型 ， 因 此 以 后 在 传递 参数 
时 使 用 Camera::Ptr 交 型 即 可 。 

5. 我 们 用 Sophus::SE3 来 表达 相机 的 位 姿 。Sophus 库 在 至 代数 一 讲 介 


绍 过 。 
在 源 文 件 中 ， 给 出 Camera 方 法 的 实现 : 


四 slambook/project/0.1/src/camera.cpp 


#include "myslam/camera.h" 


namespace myslam 


{ 


Camera: :Camera() 
{ 
d 


Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T c w ) 


{ 


return T_c_w*p_w; 


Vector3d Camera::camera2world ( const Vector3d& p c, const SE3& T_c_w ) 


Y 


return T c w.inverse() *p_c; 


Vector2d Camera::camera2pixel ( const Vector3d& p c ) 


1 
return Vector2d ( 
fz * p6 (0,0 } # Be (2,0) + &, 
fy * pic ( 1,0 ) Pe ( 2,0: ) + ey 
); 
) 


Vector3d Camera::pixel2camera ( const Vector2d& p p, double depth ) 
{ 
return Vector3d ( 
( p.p ( 0,0 )-cx_ ) *depth/fx_, 
( p.p (1,0 )-cy_ ) *depth/fy_, 
depth 
); 


Vector2d Camera: :world2pixel ( const Vector3d& p_w, const SE3& T c w ) 


37 |t 
38 return camera2pixel ( world2camera ( p_w, T_c_w ) ); 


3 |] 


4 | Vector3d Camera::pixel2world ( const Vector2d& p p, const SE3& T c w, double depth 
) 

a 1 

43 return camera2world ( pixel2camera ( p p, depth ), T c. w ); 

44 |} 

45 |} 
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9.1.4 Frame 类 


下 和 面 来 考虑 Frame 类 。Frame 类 是 基本 数据 早 元 ， 在 许多 地 方 会 用 
到 ， 但 现在 是 初期 设计 阶段 ， 我 们 还 不 清楚 以 后 可 能 新 加 的 内 容 。 所 以 
这 里 的 Frame 关 只 提供 基本 的 数据 存储 和 接口 。 如 果 之 后 有 新 增 的 内 
Zt, FRARSLTE IBBARJM e 


四 slambook/project/0.1/include/myslam/frame.h 


— = 5 T Up J = ~ ~ 4 


1 | class Frame 


zo 

3 | public: 

4 typedef std::shared_ptr<Frame> Ptr; 

5 unsigned long id ; // id of this frame 

6 double time stamp ; // when it is recorded 

7 SE3 T_c_w_; // transform from world to camera 

8 Camera::Ptr camera_; // Pinhole RGB-D Camera model 
9 Mat color_, depth_; // color and depth image 


1 | public: // data members 


12 Frame () ; 
13 Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera= 
nullptr, Mat color=Mat(), Mat depth=Mat() ); 
14 ~Frame() ; 
15 
16 // factory function 
17 static Frame::Ptr createFrame() ; 
18 
19 // find the depth in depth map 
20 double findDepth( const cv::KeyPoint& kp ); 
21 
22 // Get Camera Center 
23 Vector3d getCamCenter() const; 
24 
25 // check if a point is in this frame 
26 bool isInFrame( const Vector3d& pt world ); 
a |i 





在 Frame 中 ， 我 们 定义 了 ID、 时 间 惟 、 位 姿 、 相 机 、 图 像 这 几 个 
量 ， 这 应 该 是 一 个 帧 当中 含有 的 最 重要 的 信息 。 在 方法 中 ， 我 们 提取 了 
几 个 重要 的 方法 : 创建 Frame、 寻 找 给 定点 对 应 的 深 皮 、 获 取 相 机 光 
心 、 判 断 某 个 点 是 人 否 在 视野 内 ， 等 等 。 它 们 的 实现 是 比较 平凡 的 ， 请 读 
者 参考 frame.cpp 了 解 这 些 函 数 的 具体 实现 。 


9.1.5 “MapPoint 基 


MapPoint 表 示 路 标点 。 我 们 将 信 计 它 的 世界 坐标 ， 并 且 会 拿 当 前 帧 
提取 到 的 特征 点 与 地 图 中 的 路 标点 匹配 ， 来 们 计 相 机 的 运动 ， 因 此 还 需 
要 存储 它 对 应 的 拍 述 子 。 此 外 ， 我 们 会 记录 一 个 扣 个 观测 到 的 次 数 和 被 
匹配 的 次 数 ， 作 为 评价 其 好 坏 程度 的 指标 。 


四 slambook/project/0.1/include/myslam/frame.h 


class MapPoint 
{ 
public: 
typedef shared_ptr<MapPoint> Ptr; 
unsigned long 112 Z^ ID 
Vector3d pos. ; // Position in world 
Vector3d norm, ; // Normal of viewing direction 
descriptor ; // Descriptor for matching 
observed times, ; // being observed by feature matching algo. 


correct times. ; // being an inliner in pose estimation 


MapPoint () ; 


MapPoint( long id, Vector3d position, Vector3d norm ); 


// factory function 
static MapPoint::Ptr createMapPoint () ; 





PE, wear Hy) DUN src/map.cpp£rr A SEXW., HAERE is 
这 些 数 据 成 员 的 初始 化 问题 。 


9.1.6 Map% 
Map 类 管理 着 所 有 的 路 标点 ， 并 负责 添加 新 路 标 、 删 除 不 好 的 路 标 


等 工作 。VO 的 匹配 过 程 只 要 和 Map 打 交道 即 可 。 当 然 Map 也 会 有 很 多 操 
作 ， 但 现 阶段 我 们 只 定义 主要 的 数据 结构 。 


四 slambook/project/0.1/include/myslam/map.h 


class Map 
1 
public: 
typedef shared ptr«Map» Ptr; 


unordered_map<unsigned long, MapPoint::Ptr > map points; // all 


Landmarks 
unordered map«unsigned long, Frame::Ptr > keyframes, ; // ali key- 


frames 


Map() {} 


void insertKeyFrame( Frame::Ptr frame ); 


void insertMapPoint( MapPoint::Ptr map_point ); 





Map 类 中 实际 存储 了 各 个 关键 帧 和 路 标点 ， 既 需要 随机 访问 ， 又 需 
要 随时 插入 和 删除 ， 因 此 我 们 使 用 散 列 〈Hash) 来 进行 存储 。 


9.1.7 Con fi g% 


Con fi g 关 负责 参数 文件 的 恋 取 ， 并 在 程序 的 任意 地 方 都 可 随时 提 
供 参 数 的 值 。 所 以 我 们 把 Con fi g 写 成 单 件 模式 〈Singleton) 。 它 只 有 一 
个 全 局 对 象 ， 当 我 们 设置 参数 文件 时 ， 创 建 该 对 象 并 读 取 参数 文件 ， 随 
后 了 台 可 以 在 任意 地 方 访 问 参 数值 ， 最 后 在 程序 结束 时 目 动 销毁 


四 slambook/project/0.1/include/myslam/con fi g.h 





class Config 


l 

2 [4 

3 | private: 

4 static std::shared_ptr<Config> config. ; 

5 cv::FileStorage file, ; 

6 

7 Config O { // private constructor makes a singleton 
a | public: 

9 ~Config(); // close the file when deconstructing 


11 // set a new config file 


12 static void setParameterFile( const std::string& filename ); 
13 

14 // access the parameter values 

15 template< typename T > 

16 static T get( const std::string& key ) 

17 { 


18 return T( Config::config_->file_[key] ); 
19 } 





说 明 如 下 : 


1. 我 们 把 构造 函数 声明 为 私有 ， 防 止 这 个 美的 对 象 在 刚 处 建立 ， 卸 
只 能 在 setParameterFile 时 构造 。 实 际 构造 的 对 象 是 Con fi g 的 智能 指针 : 
static shared ptr«Con fi g»con fi g_。 用 和 留 能 指针 的 原因 是 可 以 目 动 析 
构 ， 省 得 我 们 再 调 一 个 别 的 函数 来 做 析 构 。 

2. 在 文件 读 取 方面 ， 我 们 使 用 OpenCV 提 供 的 FileStorage 类 。 它 可 以 
谈 取 一 个 YAML 文 件 ， 且 可 以 访问 其 中 任意 一 个 字段 。 由 于 参数 实质 全 
可 能 为 整数 、 浮 点 数 或 字符 串 ， 所 以 我 们 通过 一 个 模板 函数 get 来 获得 
任意 类 型 的 参数 值 。 

下 和 面 是 Con fi g 的 实现 。 注 意 ， 我 们 把 单 例 模 式 的 全 局 指针 定义 在 
此 源 文件 中 了 : 


中 slambook/project/0.1/src/con fi g.cpp 


1 |void Config::setParameterFile( const std::string& filename ) 


i [1 

3 if ( config == nullptr ) 

4 config = shared ptr«Config»(new Config); 

5 config_->file_ = cv::FileStorage( filename.c str(), cv::FileStorage::READ ) ; 
6 if ( config_->file_.isOpened() == false ) 

7 1 

8 std::cerr<<"parameter file "<<filename<<" does not exist."««std::endl; 

9 config ->file_.release() ; 

10 return ; 


2 |} 

3 | Config: :~Config() 

4 |{ 

15 if ( file .isOpened() ) 


16 file .release(); 


ü |4 
i$ | Shared_ptr<Config> Config::config_ = nullptr; 





在 实现 中 ， 我 们 只 要 判断 一 下 参数 文件 是 否 存在 即 可 。 定 义 了 这 个 
Con fi g 类 后 ， 我 们 可 以 在 任何 地 方 获取 参数 文件 里 的 参数 。 例 如 ， 妆 
想 要 定义 相机 的 焦距 f. 时 ， 按 照 如 下 步 缀 操作 即 可 : 

1. 在 参数 文件 中 加 入 : “Camera.fx:500”。 


1 |myslam::Config::setParameterFile("parameter.yaml"); 


2 |double fx = myslam::Config::get«double» ("Camera.fx"); 





REIRIS fe HHE T à 


当然 ， 参 数 文件 的 实现 方法 绝对 不 止 这 一 种 。 我 们 主要 从 程序 开发 
上 的 便利 角度 来 考虑 这 个 实现 ， 读 者 当然 也 可 以 用 更 简单 的 方式 来 实现 
参数 的 配置 。 


至 此 ， 我 们 定义 了 SLAM 程 序 的 基本 数据 结构 ， 书 号 了 右 干 个 基本 


类 。 这 好 比 是 造 房子 的 砖头 和 水 泥 。 你 可 以 调用 cmake 编 译 这 个 0.1 版 ， 

尺 官 它 还 没有 实质 性 的 功能 。 接 下 来 我 们 来 考虑 把 前 面 讲 过 的 VO 算法 

加 到 工程 中 ， 并 做 一 些 汕 斌 来 调整 各 算法 的 性 能 。 注 意 ， 笔 痢 会 刻意 地 
其 露水 些 设计 的 问题 ， 所 以 你 看 到 的 实现 不 见得 融 是 最 好 的 《或 者 足够 
好 的 ) 。 


9.2 基本 的 VO: 特征 提取 和 匹配 


下 面 我 们 来 实现 VO， 先 来 考虑 特征 点 法 。 它 的 任务 是 ， 根 据 输 入 
的 图 像 ， 计 算 相 机 运动 和 特征 点 人 位置。 前面 我 们 讨论 的 都 是 在 两 两 帧 间 
的 位 次 估计 ， 然 而 我 们 将 友 现 ， 仪 侍 两 帧 的 估计 是 不 够 的 。 我 们 会 把 特 
全 点 绥 存 成 一 个 小 地 图 ， 计 算 当 前 帧 与 地 图 之 间 的 位 置 关系 。 但 那样 程 
序 会 复杂 一 些 ， 所 以 ， 让 我 们 先 订 个 小 目标 ， 暂 时 从 两 两 帧 间 的 运动 信 
VOR. 


9.2.4 两 两 帧 的 视 党 里 程 计 


如 果 像 前 面 两 讲 一 样 ， 只 关心 两 个 帧 之 间 的 运动 估计 ， 并 且 不 优化 
特征 点 的 位 置 。 然 后 把 佑 得 的 位 姿 * 串 ?起 来 ， 也 能 得 到 一 条 运动 轨迹 。 
这 种 方式 可 以 看 成 两 两 帧 间 的 (Pairwise) 无 结构 (Structureless) 的 
VO， 实 现 起 来 最 为 简单 ， 但 是 效果 不 佳 。 为 什么 不 佳 呢 ? 我 们 一 起 来 
体验 一 下 。 记 访 工 程 为 0.2 版 本 。 


两 两 帧 之 间 的 VO 工作 示意 图 如 图 9-4 所 示 。 在 这 种 VO 里 ， 我 们 定 
义 了 参考 帧 (Reference) 和 当前 帧 〈Current) 这 两 个 概念 。 以 参考 帧 为 
坐标 系 ， 我 们 把 当前 帧 与 它 进 行 特征 匹配 ， 并 估计 运动 关系 。 假 设 参考 
帕 相 对 世界 坐标 的 变换 起 阵 为 T,， ， 当 前 帧 与 世界 坐标 系 间 为 T,，， 则 每 
估计 的 运动 与 这 两 个 帧 的 变换 窍 阵 构 成 左 滋 关系 : 


Tor, s.t. Low = LerTrw- 


Et- 1 到 t 时 刻 ， 我 们 以 t- 1 为 参考 ， 求 取 t 时 刻 的 运动 。 这 可 以 通过 
符 征 扣 匹 配 、 光 流 或 卫 接 法 得 人 到， 但 这 里 我 们 只 关心 运动 ， 不 天 心 结 
构 ”。 换 名 话说， 只 要 通过 特征 点 成 功 求 出 了 了 运动， 我们 束 不 再 需要 这 
一 巾 的 特征 点 了 。 这 种 做 法 当然 会 有 缺陷 ， 但 是 忽略 挥 数量 庞大 的 特征 
RA Lave ites. Ala, fet 到 t +HZ, REIL At 时 刻 为 参 
Gil, At 到 t +1 间 的 运动 天 系 。 如 此 往复 ， 束 得 到 了 一 条 运动 轨迹 。 
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图 9-4 两 两 帧 的 VO 示 意图 。 


这 种 VO 的 工作 方式 旦 徐 单 的 ， 不 过 实现 也 可 以 有 和 大 和 干 种 。 我 们 以 
传统 的 匹配 特征 点 一 一 求 PnP 的 方法 为 例 实现 一 退 。 布 望 读者 能 够 结合 
之 前 儿 讲 的 知识 ， 目 己 实 现 一 下 光 流 /下 接 法 或 1CP 求 运动 的 VO。 在 区 
配 特 征 皮 的 方式 中 ， 最 午 要 的 是 参考 帧 与 当前 帧 之 间 的 特征 匹配 关系 ， 
它 的 流程 可 归纳 如 下 : 


1. 对 新 来 的 当前 帜 ， 所 取 关 键 点 和 手 述 子 。 


2. 如 果 系 统 未 初始化， 以 该 巾 为 参考 帆 ， 根 据 深 怪 图 计算 关键 点 的 
3D 人 位置， 返回 第 1 步 。 
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5. 石 成功， 把 当前 帧 作为 新 的 参考 帆 ， 返 回 第 1 步 。 


6. 厂 失败 ， 计 有 录 连 续 丢 失 帆 数 。 当 连续 丢失 超过 一 定 帧 数 时 ， 置 VO 
状态 为 丢失 ， 算 法 结束 。 知 未 超过 ， 返 回 第 1 步 。 


VisualOdometry 类 给 出 了 上 述 算 法 的 实现 。 


四 slambook/project/0.2/include/myslam/visual_odometry.h 


class Visual0dometry 


1 
public: 


typedef shared ptr«VisualO0dometry» Ptr 
enum VOState 1 

INITIALIZING--1, 

OK=0, 

LOST 
}; 





》 


VOState state ; // current VO status 
Map::Ptr map_; // map with all frames and map points 
Frame::Ptr ref ; // reference frame 


Frame::Ptr curr_; // current frame 


cv::Ptr<cv::ORB> orb ; // orb detector and computer 
vector<cv::Point3f> pts 3d ref ; // 3d points in reference frame 
vector<cv: :KeyPoint> keypoints curr ; // keypoints in current frame 
Mat descriptors_curr_; // descriptor in current frame 


Mat descriptors_ref 


// descriptor in reference frame 


vector<cv::DMatch> feature matches, ; 


SE3 T. c r estimated ; // the estimated pose of current frame 
int num inliers, ; // number of inlier features in icp 


int num lost ; // number of lost times 


// parameters 


int num of features ; 


// number of features 

double scale factor ; // scale in image pyramid 

int level pyramid ; // number of pyramid levels 

float match ratio ; // ratio for selecting good matches 
int max num lost ; // max number of continuous lost times 


int min inliers ; // minimum inliers 


double key frame min rot; // minimal rotation of tuo key-frames 


double key frame min trans; // minimal translation of two key-frames 


public: // functions 


VisualOdometry() ; 
~VisualOdometry () ; 


bool addFrame( Frame::Ptr frame ); // add a new frame 


protected: 


// inner operation 

void extractKeyPoints(); 
void computeDescriptors() ; 
void featureMatching() ; 
void poseEstimationPnP () ; 
void setRef3DPoints(); 


void addKeyFrame() ; 
bool checkEstimatedPose() ; 
bool checkKeyFrame() ; 


关于 这 个 VisualOdometry 类 ， 有 几 点 需要 解释 : 


1.VO 本 号 有 香干 种 状态 : 设 定 第 一 帧 、 顺 利 跟踪 或 丢失 ， 你 可 以 把 
它 看 成 一 个 有 限 状 态 机 (Finite State Machine, FSM) 。 当 然 状态 也 可 
以 有 更 多 种 ， 例 如 ， 单 目 VO 至 少 还 有 一 个 初始 化 状态 。 在 我 们 的 实现 
中 ， 考 虑 最 简单 的 三 个 状态 : Wu. IER. XX. 

2. 我 们 把 一 些 中 间 变 量 定 义 在 类 中 ， 这 样 可 省 去 复杂 的 参数 传递 。 
因为 它们 都 古 定义 在 类 内 部 的 ， 所 以 各 个 函数 都 可 以 访问 它们 。 

3. 特 征 提取 和 匹配 当中 的 参数 从 参数 文件 中 旋 取 。 例 如 : 


1 | VisualO0dometry::VisualO0dometry() : 
2 state. ( INITIALIZING ), ref. ( nullptr ), curr_. ( nullptr ), map. ( 


new Map ), num lost ( O ), num inliers ( 0 ) 


num of features = Config::get<int> ( "number of features" ); 


scale factor, Config::get<double> ( "scale factor" ); 


level pyramid. = Config::get<int> ( "level pyramid" ); 


match ratio = Config::get<float> ( "match, ratio" ); 


el oo N O^ uni — WwW 





4.addFrame ek We Sb eh al BAe. TERA VOY, X4 ESL RARUS 
Frame 类 后 ， 调 用 addFrame 估 计 其 位 姿 。 访 函数 根据 VO 所 处 的 状态 实现 
不 同 的 操作 : 


bool VisualOdometry::addFrame ( Frame::Ptr frame ) 


1 

switch ( state. ) 

t 

case INITIALIZING: 

1 
state = OK; 
curr_ = ref_ = frame; 
map_->insertKeyFrame ( frame ); 
// extract features from first frame 
extractKeyPoints() ; 
computeDescriptors () ; 
// compute the 3d position of features in ref frame 
setRef3DPoints() ; 
break; 

} 

case OK: 

1 
curr = frame; 
extractKeyPoints(); 


computeDescriptors(); 


22 featureMatching() ; 


23 poseEstimationPnP(); 

24 if ( checkEstimatedPose() == true ) // a good estimation 

25 i 

26 curr -ST cw = Te r estimated * ref T cw; //Tcuwz-z 


I c re«T r y 


27 ref = curr_; 

28 setRef3DPoints(); 

29 num lost = 0; 

30 if ( checkKeyFrame() == true ) // is a key-frame 
31 1 

32 addKeyFrame(); 

33 } 

34 } 

35 else // bad estimation due to various reasons 
36 { 

37 num_lost_++; 

38 if ( num lost. > max num lost ) 
39 1 

40 state = LOST; 

4l } 

42 return false; 

43 7 

44 break; 

45 } 

46 case LOST: 

47 { 

48 cout<<"vo has lost."««endl; 

49 break; 

50 } 

51 } 

52 return true; 

5 |} 


值得 一 所 的 是 ， 由 于 各 种 原因 ， 我 们 设计 的 上 述 VO 算 法 ， 每 一 步 
祁 有 可 能 失败 。 例 如 ， 图 片 中 不 易 捉 特征、 特征 点 缺少 深度 值 、 误 匹 
配 、 运 动 估计 出 错 ， 等 等 。 因 此 ， 要 设计 一 个 健壮 的 VO， 必 须 ( 最 好 
EENH) 务虚 到 上 述 所 有 可 能 出 错 的 地 方 一 一 那 目 然 会 使 程序 变 得 非 


第 复 洒 。 我 们 在 checkEstimatedPose 中 根据 内 点 Cinlier? 的 数量 及 运动 
的 大 小 做 一 个 简单 的 检测 : 认为 内 点 不 可 太 少 ， 而 运动 不 可 能 过 大 。 当 
然 ， 读 者 也 可 以 思考 其 他 检测 问题 的 手段 ， 壬 试 一 下 效 果 。 

我 们 略 去 VisualOdometry 类 其 余 的 实现 ， 读 者 可 在 GitHub 上 找到 所 
有 的 源 代 码 。 最 后 ， 我 们 在 test 中 加 入 该 VO 的 测试 程序 ， 使 用 数据 集 观 
察 估计 有 的 运动 效 末 : 


四 slambook/project/0.2/test/run vo.cpp 


43 


int main ( int argc, char** argv ) 


1 


if ( argc != 2) 
1 
cout<<"usage: run vo parameter file"««endl; 


return 1; 


myslam::Config::setParameterFile ( argv[1] ); 


myslam::VisualOdometry::Ptr vo ( new myslam::VisualÜOdometry ); 


string dataset dir = myslam::Config::get<string> ( "dataset dir" ); 
cout<<"dataset: "««dataset dir««endl; 
ifstream fin ( dataset dir*"/associate.txt" ); 
if ( !fin ) 
1 
cout<<"please generate the associate file called associate.txt!"««endl; 


return 1; 


vector<string> rgb files, depth files; 

vector<double> rgb times, depth times; 

while ( !fin.eof() ) 

1 
string rgb time, rgb file, depth time, depth file; 
fin>>rgb_time>>rgb_file>>depth_time>>depth_file; 
rgb times.push back ( atof ( rgb time.c str() ) ); 
depth times.push, back ( atof ( depth time.c str() ) ); 
rgb files.push back ( dataset dir*"/"*rgb file ); 
depth files.push, back ( dataset dir*"/"*depth file ); 


if ( fin.good() == false ) 


break; 


myslam::Camera::Ptr camera ( new myslam::Camera ); 


// visualization 

cv::viz::Viz3d vis("Visual Odometry"); 

cv::viz::WCoordinateSystem world, coor(1.0), camera, coor(0.5); 

cv::Point3d cam pos( O, -1.0, -1.0 ), cam focal, point(0,0,0), cam y dir(0,1,0); 
cv::Affine3d cam pose = cv::viz::makeCameraPose( cam pos, cam focal point, 
cam, y dir ); 


vis.setViewerPose( cam, pose ); 


75 


76 


77 


78 


79 


80 


81 


82 


83 


84 


world coor.setRenderingProperty(cv::viz::LINE WIDTH, 2.0); 


camera coor.setRenderingProperty(cv::viz::LINE WIDTH, 1.0); 


vis.showWidget( "World", world coor ); 


vis.showWidget( "Camera", camera coor ); 


cout<<"read total "««rgb files.size() <<" entries"««endl; 


for ( int i=0; i«rgb files.size(); i++ ) 


1 


Mat 
Mat 


color = cv::imread ( rgb files[i] ); 


depth = cv::imread ( depth files[il, -1 ); 


if ( color.data--nullptr || depth.data--nullptr ) 


break; 


myslam::Frame::Ptr pFrame - myslam::Frame::createFrame(); 


pFrame->camera_ = camera; 


pFrame-»color = color; 


pFrame->depth_ = depth; 


pFrame->time_stamp_ = rgb times[i]; 


boost::timer timer; 


vo->addFrame ( pFrame ); 


cout<<"VO costs time: "««timer.elapsed()««endl; 


if ( vo->state_ == myslam::VisualOdometry::LOST ) 


break; 


SE3 Tcw = pFrame-?T c w .inverse(); 


// show the map and the camera pose 


CV:: 


CV: 


CV: 


Affine3d M( 

cv: :Affine3d: :Mat3( 
Tcw.rotation_matrix() (0,0), Tcw.rotation_matrix() (0,1), Tew. 
rotation_matrix() (0,2), 
Tcw.rotation_matrix() (1,0), Tcw.rotation_matrix() (1,1), Tew. 
rotation_matrix() (1,2), 
Tcw.rotation_matrix() (2,0), Tcw.rotation_matrix() (2,1), Tew. 


rotation_matrix() (2,2) 


cv: :Affine3d: : Vec3( 
Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation() 
(2,0) 


:imshow("image", color ); 
:waitKey(1); 
.setWidgetPose( "Camera", M); 


85 vis.spinOnce(1, false); 


86 } 
87 return 0; 
as |} 





为 了 运行 这 个 程序 ， 需 要 做 几 件 事 : 

1. 因 为 我 们 用 OpenCV «viz AN ST, TTR REN 
是 OpenCV 3， 并 且 viz 模 块 也 已 编译 安装 。 

2. 准 备 TUM 数 据 集 中 的 其 中 一 个 。 人 简单 起 见 ， 笔 者 推荐 fr1_xyz 那 一 
个 。 请 使 用 associate.py 生 成 一 个 配对 文件 associate.txt。 关 于 TUM 数 据 集 
格 云 ， 在 8.3 节 中 已 介绍 。 

3. 在 con fi gdefault.yaml 中 岳 写 你 的 数据 集 所 在 路 径 ， 参 照 笔 者 的 写 
法 即 可 。 然 后 ， 用 


1 | bàn/run vo config/default.yaml 


执行 程序 ， 束 可 以 看 到 实时 的 演示 了 ， 如 图 9-5 所 示 。 
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图 9-5 02 版 本 的 VO 演示 。 





在 演示 程序 中 ， 你 可 以 看 到 当前 帧 的 图 像 与 它 的 估计 位 置 。 我 们 男 
出 了 世界 坐标 系 的 坐标 轴 〈( 大 ) 与 当前 帆 的 坐标 轴 〈 小 )， 闫 色 与 轴 的 
对 应 关系 为 : 蓝 色 一 Z， 红 色 一 X， 绿 色 一 Y。 你 可 以 直观 地 感受 到 相机 
的 运动 ， 它 与 我 们 人 类 的 感觉 是 大 致 相符 的 ， 尽 管 效 果 距 离 预 想 还 有 一 
定 的 到 距 。 程 序 还 输出 了 VO 单 次 计算 的 用 时 ， 在 笔者 的 机 右上 ， 能 够 
以 每 次 花费 30ms 左 右 的 速度 运行 。 减 少 特征 点 数量 可 以 提高 运算 速度 。 
读者 可 以 修改 运行 参数 和 数据 集 ， 看 看 它 在 各 种 情况 下 的 表现 。 


922 ”讨论 


本 市 ， 我 们 实现 了 一 个 人 简 里 的 两 两 帧 间 的 视 和 里 程 计 ， 然 而 不 官 从 
速度 还 十 精 度 上 来 说 ， 它 的 效果 部 不 理想 。 似 乎 这 种 看 似 蚀 持 的 思路 并 
不 能 得 到 很 好 的 结 来 。 我 们 来 考虑 一 下 有 哪些 可 能 的 原因 : 


1. 在 位 姿 估 计时， 我 们 使 用 了 RANSAC 求 出 的 PnP 解 。 由 于 
RANSAC 只 采用 少数 儿 个 随机 点 来 计算 PnP， 虽 然 能 够 确定 inlier， 但 议 
方法 容易 受 噪声 影响 。 在 3D-2D 点 存在 噪声 的 情形 下 ， 我 们 要 用 
RANSAC 的 解 作为 初 值 ， 再 用 非 线 性 优化 求 一 个 最 优 值 。 下 一 节 将 说 明 
这 种 做 法 是 优 于 现在 的 做 法 的 。 


2. 由 于 现在 的 VO 是 无 结构 的 ， 特 征 点 的 3D 位 置 被 当 作 真 值 来 估计 
运动 。 但 实际 上 ，RGB-D 的 深度 图 必定 和 带 有 一 定 的 误 闪 ， 特 别 是 那些 深 
上 度 过 近 或 过 远 的 地 方 。 并 且 ， 由 于 特征 点 往往 位 于 物体 的 边缘 处 ， 那 些 
地 方 的 深度 测量 值 通常 是 不 准确 的 。 所 以 现在 的 做 法 不 够 精确 ， 我 们 需 
要 把 特征 点 也 放 在 一 起 优化 。 


3. 只 知情 参考 帧 /当前 帧 的 方式 ， 一 方面 使 得 位 姿 售 计 过 于 依 顿 参 考 
帧 。 如 各 参考 帧 质量 太 兰 ， 比 如 ， 出 现 严 重 巡 挡 、 光 照 变化 的 情况 下 ， 
跟 踩 容易 丢失 。 并 且 ， 当 参考 帧 位 姿 佑 计 不 准时 ， 还 会 出 现 明 旺 的 深 移 
。 历 一 方面 ， 仅 使 用 两 怖 数据 显然 没有 友 分 地 利用 所 有 的 信息 。 更 目 然 
的 方 却 征 比 较 当 前 帧 和 地 图 ， 而 不 是 比较 当前 帧 和 参考 帧 。 于 征 ， 我 们 
要 关心 如 何 把 当前 帧 与 地 图 进行 匹配 ， 以 及 如 何 优化 地 图 点 的 问题 。 


4. 由 于 输出 了 各 步骤 的 运行 时 间 ， 我 们 可 以 对 计算 量 有 一 个 大 概 的 
了 解 〈 表 9-1) 。 


表 9-1 茶 一 次 循环 的 各 步骤 用 时 


JH | 特征 提取 | 描述 子 计算 | 特征 匹配 | PnP 求解 | 其 他 | 总 计 
as 0.0319 


可 以 看 人 到， 特征 点 的 提取 和 匹配 占用 了 绝 大 多 数 的 计算 时 间 ， 而 看 
似 复 杂 的 PnP 优化 ， 计 算 量 与 之 相 比 基本 可 以 忽略 。 因 此 ， 如 何 提高 特 
征 提取 、 匹 配 算法 的 速度 ， 将 是 特征 点 方法 的 一 个 重要 的 主题 。 一 种 可 
预见 的 方式 是 使 用 卫 接 法 / 光 流 ， 可 有 效 地 避 开 莹 午 的 特征 计算 工作 。 
本 书 之 前 已 讨论 过 生 接 法 和 交流 法 ， 读 者 个 妨 目 行 符 试 一 下 。 





9.3 A: 优化 PnP 的 结 


接 下 来 ， 我 们 沿 看 之 前 的 内 容 ， 壬 试 一 些 改 进 VO 的 方法 。 本 市 
中 ， 我 们 来 尝试 RANSAC PnP 加 上 和 迭代 优化 的 方式 估计 相机 位 姿 ， 看 看 
是 否 对 前 一 节 的 效果 有 所 改进 。 

非 线性 优化 问题 的 求解 ， 已 经 在 第 6 讲 和 第 7 讲 介绍 过 了 。 由 于 本 节 
的 目标 是 估计 位 姿 而 非 结 构 ， 我 们 以 相机 位 姿 E 为 优化 变量 ， 通 过 最 小 
化 重 投影 误差 来 构建 优化 问题 。 与 之 前 一 样 ， 我 们 自 定 义 一 个 g2o 中 的 
优化 边 。 它 只 优化 一 个 位 姿 ， 因 此 是 一 个 一 元 边 。 


四 slambook/project/0.3/include/myslam/g20 types.h 





1 | class EdgeProjectXYZ2UVPoseOnly: public g20o::BaseUnaryEdge«2, Eigen::Vector2d, g2o 

::VertexSE3Expmap > 

{ 

public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


virtual void computeError(); 


virtual void linearizeÜplus(); 


Mo oc “I Cn vI + U N 


virtual bool read( std::istream& in ){} 


10 virtual bool write(std::ostream& os) const {}; 


12 Vector3d point, ; 


13 Camera* camera. ; 





FEE AAALAC EN lo va eT a RECS VA ZU 
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四 slambook/project/0.3/src/g20. types.cpp 


1 | void EdgeProjectXYZ2UVPoseOnly: : computeError () 
E! 
3 | const g20::VertexSE3Expmap* pose = static cast«const g20::VertexSES3Expmap*^ 人 


_vertices[0] ); 


4 _error = measurement - camera -»camera2pixel ( 
5 pose->estimate() .map(point_) 

6 1i 

ais 


9 | void EdgeProjectXYZ2UVPoseOnly::linearizeÜplus() 


10 | { 

1 g20::VertexSE3Expmap* pose = static cast«g20::VertexSE3Expmap*» ( _vertices[0] 
); 

12 g20::SE3Quat T ( pose->estimate() ); 

13 Vector3d xyz trans = T.map ( point. ); 

14 double x = xyz trans[0]; 

15 double y = xyz trans[1]; 

16 double z = xyz trans[2]; 

17 double z 2 = z*z; 


19 .jacobianÜplusXi x*y/z 2 *camera_->fx_; 

20 _jacobian0plusXi = ( 1+ ( x*x/z 2 ) ) *camera_->fx_; 
21 _jacobian0plusXi y/z * camera_->fx_; 

22 _jacobian0plusXi "l1.fz * canera -»Px : 

23 _jacobian0plusxi 0; 


24 _jacobianOplusXi x/z_2 * camera_->fx_; 


26 _jacobian0plusXi ( 1*y*y/z 2 ) *camera_->fy_; 
27 _jacobian0plusxi -x*y/z 2 *camera_->fy_; 

28 _jacobianOplusXi -x/z *camera_->fy_; 

29 _jacobianOplusXi 0; 


30 .jacobianÜplusXi -1./z *camera_->fy_; 


31 _jacobianOplusXi y/z_2 *camera_->fy_; 





然后 ， 在 之 前 的 PoseEstimationPnP 函 数 中 ， 修 改 成 以 RANSACPnP 
结果 为 初 值 ， 再 调用 g2o 进 行 优 化 的 形式 : 


l 


四 slambook/project/0.3/src/visual_odometry.cpp 


void Visual0dometry: : poseEstimationPnP () 


{ 


// using bundle adjustment to optimize the pose 
typedef g20::BlockSolver<g2o: :BlockSolverTraits<6,2>> Block; 


Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block:: 


PoseMatrixType>() ; 


Block* solver_ptr = new Block( linearSolver ); 


g20: 


:ÜptimizationAlgorithmLevenberg* solver = new g2o:: 


ÜptimizationAlgorithmLevenberg ( solver ptr ); 


g20: 


:SparseÜptimizer optimizer; 


optimizer.setAlgorithm ( solver ); 


g20: 


:VertexSE3Expmap* pose = new g20o::VertexSE3Expmap(); 


pose->setId ( 0 ); 
pose->setEstimate ( g20::SE3Quat ( 


) dis 


T_c_r_estimated_.rotation_matrix(), T_c_r_estimated_.translation() 


optimizer.addVertex ( pose ); 


// edges 


for 


{ 


( int i=0; i<inliers.rows; i++ ) 


int index = inliers.at<int>(i,0); 


// 3D -> 2D projection 


24 EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly(); 


25 edge->setId(i); 

26 edge->setVertex(0, pose); 

27 edge->camera_ = curr_->camera_.get(); 

28 edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z ); 
29 edge->setMeasurement ( Vector2d(pts2d[index].x, pts2d[index].y) ); 
30 edge->setInformation( Eigen::Matrix2d::Identity() ); 

31 optimizer.addEdge( edge ); 

32 } 

33 

34 optimizer.initializeÜptimization(); 

35 optimizer.optimize(10); 

36 

37 T c r estimated = SE3 ( 

38 pose->estimate().rotation(), 

39 pose-»estimate().translation() 


请 读者 运行 此 程序 ， 对 比 之 前 的 结果 。 你 将 发 现 估 计 的 运动 明显 稳 
定 了 和 很多。 同时， 由 于 新 增 的 优化 仍 是 无 结构 的 ， 规 模 很 小 ， 对 计算 时 
间 的 影响 基本 可 以 忽略 不 计 。 整 体 的 视 党 里程 计 计算 时 间 仍 在 30ms 左 
di. 

讨论 

RIEM, SD AXE. ph RB EE 
RANSAC PnP HAN fer. IRE MK BERI P8 Pte fe, 1E 
得 到 的 运动 却 更 加 准确 、 平 稳 。 从 这 次 改进 中 我 们 看 到 了 优化 的 重要 
性 。 不 过 ，0.3 版 本 的 VO 仍 受 两 两 帆 回 逻 配 的 局 限 性 影 响 。 一 旦 视频 序 
列 当中 某 个 帧 丢失 ， 就 会 导致 后 续 的 帧 也 无 法 和 上 一 帧 匹配 。 下 面 ， 我 
们 把 地 图 引入 到 VO 中 来 。 


94 改进 : 局 部 地 图 


本 节 我 们 将 VO 匹配 到 的 特征 点 放 到 地 图 中 ， 并 将 当前 帧 与 地 图 点 
进行 匹配 ， 计 算 位 姿 。 这 种 做 法 与 之 前 的 差异 如 图 9-6 所 示 。 

在 两 两 帧 间 比较 时 ， 我 们 只 计算 参考 帧 与 当前 帧 之 间 的 特征 匹配 和 
运动 关系 ， 在 计算 之 后 把 当前 帧 设 为 新 的 参考 帧 。 而 在 使 用 地 图 的 VO 
中 ， 每 个 帧 为 地 图 贡献 一 些 信息 ， 比 如 ， 添 加 新 的 特征 点 或 更 新 旧 特征 
点 的 位 置 估计 。 地 图 中 的 特征 点 位 置 往往 是 使 用 世界 坐标 的 。 因 此 ， 当 
前 帧 到 来 时 ， 我 们 求 它 和 地 图 之 间 的 特征 匹配 与 运动 关系 ， 即 直接 计算 
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图 9-6 PSybIVO- 3b RVOR. 


这 样 做 的 好 处 是 ， 我 们 能 够 维护 一 个 不 断 更 新 的 地 图 。 只 要 地 图 是 
正确 的 ， 即 使 中 间 某 帧 出 了 差错 ， 仍 有 希望 求 出 之 后 那些 帧 的 正确 位 


置 。 请 注意 ， 我 们 现在 还 没有 详细 地 讨论 SLAM 的 建 图 问题 ， 所 以 这 里 
的 地 图 仪 是 一 个 临时 性 的 概念 ， 指 的 是 把 各 帧 特征 点 缓存 到 一 个 地 方 构 
成 的 特征 点 的 集合 。 


地 图 又 可 以 分 为 局 部 (Local) 地 图 和 全 局 CGlobaD 地 图 两 种 ， 
由 于 用 途 不 同 ， 往 往 分 开 讨论 。 顾 名 思 义 ， 局 部 地 图 摘 述 了 附近 的 特征 
Riek 我 们 只 保留 离 相机 当前 位 置 较 近 的 特征 点 ， 而 把 远 的 或 视野 
外 的 特征 点 丢 挥 。 这 些 特 征 点 是 用 来 和 当前 帜 匹配 来 求 相机 位 置 和 的， 所 
以 我 们 希望 它 能 够 做 得 比较 快 。 男 一 方面 ， 全 局 地 图 则 记录 了 从 SLAM 
运行 以 来 的 所 有 特征 点 。 显 然 它 的 规模 要 大 一 些 ， 主 要 用 来 表达 整个 环 
境 ， 但 是 直接 在 全 局 地 图 上 定位 ， 对 计算 机 的 负担 就 太 大 了 。 它 主要 用 
于 回环 检测 和 地 图 表达 。 


在 视觉 里 程 计 中 ， 我 们 更 关心 可 以 下 接 用 于 定位 的 局 部 地 图 (如果 
决心 要 用 地 图 的 话 ) 。 所 以 本 讲 我 们 来 维护 一 个 局 部 地 图 。 随 看 相机 运 
动 ， 我 们 往 地 图 里 添加 新 的 特征 点 。 我 们 仍然 要 提醒 读者 : Au 
图 取决 于 你 对 精度 一 效 这 这 个 矛盾 的 把 握 。 我 们 完全 可 以 出 于 效率 的 考 
量 ， 使 用 两 两 无 结构 式 的 VO; 也 可 以 为 了 更 好 的 精度 ， 构 建 局 部 地 图 
旋 至 考虑 地 图 的 优化 。 

局 部 地 图 的 一 件 抵 烦 事 是 维护 它 的 规模 。 为 了 你 证 实时 性 ， 我 们 十 
要 你 证 地 图 规模 不 全 于 太 大 《人 否则 匹配 会 消耗 大 量 的 时 间 ) © Gab, A 
个 帧 与 地 图 的 特征 匹配 存在 看 一 些 加 速 手段 ， 但 由 于 技术 上 比较 复杂 ， 
我 们 的 例 程 中 残 不 给 出 了 。 

现在 ， 来 实现 地 交点 关 吧 。 我 们 稍 加 完善 之 前 没有 用 到 的 MapPoint 
R, dI ES BO E GER AC e 

由 slambook/project/0.4/include/myslam/mappoint.h 





— 


class MapPoint 
1 
public: 
typedef shared ptr«MapPoint» Ptr; 
unsigned long id; // ID 
static unsigned long factory_id_; // factory id 
bool good_; // wheter a good point 
Vector3d pos_; // Position in world 
Vector3d norm_; // Normal of viewing direction 


Mat descriptor_; // Descriptor for matching 


list<Frame*> observed frames ; // key-frames that can observe this point 


int matched_times_; // being an inliner in pose estimation 


int visible times ; // being visible in current frame 


MapPoint () ; 
MapPoint ( 
unsigned long id, 
const Vector3d& position, 
const Vector3d& norm, 
Frame* frame=nullptr, 
const Mat& descriptor=Mat () 
); 


inline cv::Point3f getPositionCV() const { 


return cv::Point3f( pos (0,0), pos (1,0), pos. (2,0) ); 


static MapPoint::Ptr createMapPoint(); 
static MapPoint::Ptr createMapPoint ( 
const Vector3d& pos world, 
const Vector3d& norm_, 
const Mat& descriptor, 


Frame* frame 


主要 有 的 修改 在 VisualOdometry 类 上 。 由 于 工作 流程 的 改变 ， 我 们 修 
改 了 它 的 几 个 主要 图 数 ， 例 如 ， 每 次 循环 中 要 对 地 图 进行 增删 、 统 计 每 
个 地 疼 点 被 观 汕 到 的 次 数 ， 等 等 扣 。 这 些 事情 是 比较 琐碎 的 ， 所 以 我 们 
还 是 建议 谈 者 仔细 看 看 GitHub 上 提供 的 源 代 码 。 重 点 观察 以 下 几 项 : 

1. 在 提取 第 一 帆 的 特征 点 之 后 ， 将 第 一 帆 的 所 有 特征 点 全 部 放 入 地 
图 中 : 


void VisualOdometry: :addKeyFrame () 


{ 





if ( map_->keyframes_.empty() ) 
i 
// first key-frame, add all 3d points into map 
for ( size t i=0; i«keypoints curr .size(); i++ ) 
1 
double d = curr_->findDepth ( keypoints curr [i] ); 
if (dad <0) 
continue; 
Vector3d p_world = ref_->camera_->pixel2world ( 
Vector2d ( keypoints curr [il.pt.x, keypoints curr [il.pt.y 
), curr ->T 6 * , d 
); 
Vector3d n = p world - ref -»getCamCenter(); 


n.normalize(); 


MapPoint::Ptr map point = MapPoint::createMapPoint( 


p.world, n, descriptors curr .row(i).clone(), curr_.get() 
F 


map_->insertMapPoint( map_point ); 


} 
map_->insertKeyFrame ( curr_ ); 


ret = curr; 


2 Je Zee tA, 4E H OptimizeMap A BOW HEAT Dos Bldt M 
不 在 视野 内 的 点 ， 在 匹配 数量 减少 时 添加 新 点 ， 等 等 。 





void Visual0dometry: :optimizeMap() 


// remove the hardly seen and no visible points 


for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_. 
end(); ) 
{ 


if ( !curr_->isInFrame(iter->second->pos_) ) 
í 
iter = map_->map_points_.erase(iter); 


continue; 


10 } 
1 float match ratio = float(iter-»5second-^»matched times )/iter-» 


second-»visible times. ; 


12 if ( match ratio < map point erase ratio, ) 
13 i 

14 iter = map. -»map. points, .erase(iter); 

15 continue; 

16 F 

17 double angle = getViewAngle( curr_, iter->second ); 
18 if ( angle > M_PI/6. ) 

19 { 

20 iter = map_->map_points_.erase(iter); 

21 continue; 

22 f 

23 if ( iter->second->good_ == false ) 

24 t 

25 // TODO try triangulate this map point 
26 } 

27 itert+; 

28 } 

29 

30 if ( match_2dkp_index_.size()<100 ) 

31 addMapPoints () ; 

32 if ( map_->map_points_.size() > 1000 ) 

33 { 

34 // TODO map is too large, remove some one 
35 map_point_erase_ratio_ += 0.05; 

36 } 

37 else 

38 map_point_erase_ratio_ = 0.1; 

39 cout<<"map points: "<<map_->map_points_.size()<<end1; 
40 |} 


我 们 刻意 留 空 了 一 些 地 方 ， 请 感 兴趣 的 读者 目 行 完成 。 例 如 ， 你 可 
以 使 用 三 角 化 来 更 新 特征 点 的 世界 坐标 ， 或 者 考虑 更 好 地 动态 管理 地 图 
规模 的 打上 略 。 这 些 问 题 部 是 开放 性 的 。 


3. 特 征 匹 配 代 人 码 。 匹 配 之 前 ， 我 们 从 地 图 中 拿 出 一 些 候选 把 〈 出 现 
在 视野 内 的 点 ) ， 然 后 将 它们 与 当前 师 的 特征 描述 子 进 行 匹 配 。 





void VisualO0dometry::featureMatching() 


1 


boost::timer timer; 
vector<cv::DMatch> matches; 
// select the candidates in map 


Mat desp map; 


7 Vector<MapPoint : :Ptr> candidate; 


8 for ( auto& allpoints: map_->map_points_ ) 

9 1 

10 MapPoint::Ptr& p = allpoints.second; 

T // check if p in curr frame image 

12 if ( curr_->isInFrame(p->pos_) ) 

13 1 

14 // add to candidate 

15 p-^visible times -**; 

16 candidate.push back( p ); 

17 desp_map.push_back( p->descriptor_ ); 

18 } 

19 } 

20 

21 matcher_flann_.match ( desp_map, descriptors_curr_, matches ); 
22 // select the best matches 

23 float min_dis = std::min_element ( 

24 matches.begin(), matches.end(), 

25 [] ( const cv::DMatch& mi, const cv::DMatch& m2 ) 
26 1 

27 return mí.distance < m2.distance; 

28 } )->distance; 

29 

30 match_3dpts_.clear() ; 

31 match_2dkp_index_.clear() ; 

32 for ( cv::DMatch& m : matches ) 

33 1 

34 if ( m.distance < max<float> ( min dis*match ratio , 30.0 ) ) 
35 1 

36 match 3dpts .push, back( candidate[m.queryIdx] ); 
37 match, 2dkp index, .push, back( m.trainIdx ); 

38 } 

39 } 

40 cout<<"good matches: "<<match_3dpts_.size() ««endl; 
4l cout««"match cost time: "««timer.elapsed() ««endl; 

42 |} 
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的 概 


仿 。 天 键 巾 在 证 多 视 先 SLAM 中 痢 会 用 a 到， 不 过 这 个 概念 主要 是 给 后 响 
用 的 ， 所 以 我 们 在 后 面 几 讲 再 讨论 对 关键 帧 的 评 细 人 处理。 在 实践 中 ， 我 
们 肯定 不 希望 对 每 个 图 像 都 做 详细 的 优化 和 回环 检测 ， 那 样 伴 葛 太 耗 锅 
和 资源。 至 少 相 机 搁 在 原 地 不 动 时 ， 我 们 不 希望 整个 模型 《地 图 也 好 、 轨 
迹 也 好 ) 变 得 越 来 越 大 。 因 此 ， 后 疾 优 化 的 主要 对 象 束 古 关 键 帧 。 

关键 帧 是 相机 运动 过 程 当中 茶几 个 特殊 的 帧 ， 这 里 “特殊 ”的 意义 十 
可 以 由 我 们 目 己 指定 的 。 篆 见 的 做 法 时 ， 每 当 相 机 运动 经 过 一 定 间 陨 ， 
就 取 一 个 新 的 关键 帧 并 保存 起 来 9 。 这 些 关 键 帧 的 位 姿 将 被 仔细 优化 ， 
而 位 于 两 个 关键 帧 之 间 的 那些 东西 ， 除 了 对 地 图 页 献 一 些 地 图 点 外 ， 束 
FE A SATANAE TS D. 
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备 。 现 在 ， 读 者 可 以 编译 这 个 工程 ， 看 看 它 的 运行 结果 。 本 节 的 例 程 会 
把 局 部 地 图 的 点 投影 到 网 像 平 面 并 显示 出 来 。 如 果 位 姿 估计 正确 ， 它 们 
看 起 来 应 该 像 是 固定 在 空间 中 一 样 ”。 反 之 ， 如 果 你 感觉 到 某 个 特征 点 
不 目 然 地 运动 ， 那 可 能 是 相机 位 姿 估 计 不 够 准确 ， 或 特征 点 的 位 置 不 够 
准确 。 

我 们 在 0.4 版 没有 提供 对 地 图 的 优化 ， 建 议 读者 目 行 和 尝试 一 下 。 用 
到 的 原理 主要 是 最 小 二 乘 和 三 角 化 ， 在 前 两 讲 已 介绍 过 ， 不 会 太 困难 。 
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图 9-7 ”0.4 版 VO 的 运行 截图 ， 在 两 个 不 同时 刻 标 记 了 路 标 投影 点 。 


95 小结 


YEA SEER, AiR uiu MASP aR SEL f A E A oe RET, 
为 的 是 让 读者 对 前 面 两 讲 介 绍 的 算法 有 一 个 经 验 上 的 认识 。 如 果 没 有 本 
讲 ， 你 很 难 亲 身体 会 到 例如 “特征 点 的 VO 大 约 能 够 实时 处 理 多 少 个 ORB 
特征 点 ”这 样 的 问题 向 ”。 我 们 看 到 ， 视 党 里 程 计 能 够 估算 局 部 时 间 内 的 
相机 运动 及 特征 点 的 位 置 ， 但 是 这 种 局 部 的 方式 有 明显 的 缺点 : 


LEDER. “HER, RITZA SAAP HIR” RFEA 
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2. 轨 迹 深 移 。 主 要 原因 是 每 次 估计 的 误 天 会 宗 积 全 下 一 次 估计 ， 导 
致 长 时 间 轨 迹 不 准 硝 。 大 一 点 的 局 部 地 图 可 以 缓解 这 种 现象 ， 但 它 始终 
JETTE HY o 


值得 一 提 的 是 ， 如 果 只 关心 短 时 间 内 的 运动 ， 或 者 VO 的 精度 已 经 
满足 应 用 需求 ， 那 么 有 时 候 你 可 能 需要 的 仅仅 束 是 一 个 视 党 里 程 计 ， 而 
不 用 完全 的 SLAM。 比 如 ， 某 些 无 人 机 控制 或 AR 游戏 的 应 用 中 ， 用 户 并 
不 需要 一 个 全 局 一 致 的 地 几 ， 那 么 轻便 的 VO 可 能 是 更 好 的 选择 。 不 
过 ， 本 书 的 目标 是 介绍 整个 SLAM， 所 以 我 们 还 要 走 得 更 远 一 些 ， 看 看 
后 端 和 回环 检测 是 如 何 工 作 的 。 

习题 

1. 本 书 使 用 的 C++ 技 巧 你 都 看 懂 了 吗 ? 如果 有 不 明白 的 地 方 ， 使 用 
搜索 引擎 补习 相关 的 知识 ， 包 括 : 基于 范围 的 for 循 环 、lambda 表 达 式 、 
智能 指针 、 设 计 模 式 中 的 单 例 模式 ， 等 等 。 

2. 在 0.3 版 或 0.4 版 的 基础 上 ， 添 加 对 地 图 进行 优化 的 代码 。 或 者 ， 
也 可 以 根据 PnP 结 末 做 一 下 三 角 化 ， 消 际 RGB-D 深 虐 值 的 误 产 。 


3. 观 察 本 讲 代 人 码 是 如 何 处 理 误 匹配 的 。 什 么 是 RANSAC? 阅读 文献 
[61] 或 搜索 相关 资料 进行 了 解 。 











[1] 左下 古 笔者 的 练习 作品 。 右 下 来 自 Epicwork 团 队 人 作品: 《圆明园 》。 
[2] 当然 ， 从 C++ 设计 角度 来 说 ， 保 留 之 前 的 方式 并 使 用 继承 会 更 有 效 地 复 用 现 有 代码。 


[3] 这 在 李 代 数 上 很 容易 实现 ， 请 想 想 怎么 实现 


[4] 当然 这 取决 于 你 的 机 磊 的 性 能 。 
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主要 目标 

1. 理 解 后 问 的 概念 。 

2. 理 解 以 EKEF 为 代表 的 滤波 器 后 端 工 作 原 理 。 

3. 理 解 非 线 性 优化 的 后 端 ， 明 白 稀 疏 性 是 如 何 利 用 的 。 

4. 使 用 g20 和 Ceres 实 际 操 作 后 端 优 化 。 

本 讲 开 始 ， 我 们 转 入 SLAM 系 统 的 男 一 个 重要 模块 : 后 端 优化 。 

我 们 看 到 ， 前 端 视觉 里 程 计 能 给 出 一 个 短 时 间 内 的 轨迹 和 地 图 ， 但 
由 于 不 可 避免 的 误差 累积 ， 这 个 地 图 在 长 时 间 内 是 不 准确 的 。 所 以 ， 在 
视 党 里 程 计 的 基础 上 上， 我们 还 希望 构建 一 个 尺度 、 规 模 更 大 的 优化 问 
题 ， 以 考虑 长 时 间 内 的 最 优 轨 迹 和 地 图 。 不 过 ， 考 虑 到 精度 与 性 能 的 平 
衡 ， 实 际 当中 存在 着 许多 不 同 的 做 法 。 
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10.1 概述 
10.1.1 状态 估计 的 概率 解释 


第 2 讲 中 提 到 ， 视 党 里 程 计 只 有 短暂 的 记忆 ， 而 我 们 硕 望 整个 运动 
轨迹 在 较 长 时 间 内 都 能 保持 最 优 的 状态 。 我 们 可 能 会 用 最 新 的 知识 ， 更 
新 较 久 远 的 状态 站 在 “久远 的 状态 ”的 角度 上 看 ， 仿 佛 是 未 来 的 信息 
告诉 它 “ 你 应 该 在 哪里 *"。 所 以 ， 在 后 端 优化 中 ， 我 们 通常 考虑 一 段 更 长 
时 间 内 《或 所 有 时 间 内 ) 的 状态 估计 问题 ， 而 且 不 仅 使 用 过 去 的 信息 更 
新 目 己 的 状态 ， 也 会 用 未 来 的 信息 来 更 新 目 己 ， 这 种 处 理 方 式 不 妨 称 
AHH” (Batch) 。 人 否则 ， 如 果 当 前 的 状态 只 由 过 去 的 时 刻 诀 定 ， 

其 至 只 由 前 一 个 时 刻 决 定 ， 那 不 妨 称 为 “ 潮 进 的 ”(Incremental)。 

我 们 已 经 知道 SLAM 过 程 可 以 由 运动 方程 和 观测 方程 来 描述 。 那 
么 ， 假 设 在 t=0 到 t=N 的 时 间 内 ， 有 位 姿 x, 到 xv ， 并 且 有 路 标 y，，yv 。 
按照 之 前 的 写法 ， 运 动 和 观测 方程 为 





"p = Ck—1, U + Wk 
| k = f (Tk-1, Uk) k k —1,..., N, j =La, M. (10.1) 


Bug = hU SR) Os; 


Em LL RIL: 

1. 观 测 方程 中 ， 只 有 当心 看 到 了 y WN, Aree wee, 6 USC 
没有 。 事 实 上 ， 在 一 个 位 置 通 钊 只 能 看 到 一 小 部 分 路 标 。 而 且 ， 由 于 视 
觉 SLAM 特 征 点 数量 众多 ， 上 所 以 实际 当中 观测 方程 的 数量 会 远 远 大 于 运 
动 方程 。 

2. 我 们 可 能 没有 测量 运动 的 北 置 ， 所 以 也 可 能 没有 运动 方程 。 在 这 
个 情况 下 ， 有 在 干 种 处 理 方 式 : 认为 确实 没有 运动 方程 ， 或 假设 相机 不 
动 ， 或 假设 相机 勾 速 运动 。 这 几 种 方式 都 是 可 行 的。 在 没有 运动 方程 的 
情况 下 ， 整 个 优化 问题 束 只 由 许多 个 观测 方程 组 成 。 这 就 非常 类 似 于 
SfM (Structure from Motion) 问题， 相当 于 我 们 退 过 一 组 图 像 来 恢复 运 


动 和 结构 。 与 StM 中 不 同 的 是 ，SLAM 中 的 图 像 有 时 间 上 的 先后 顺序 ， 
而 SfM 中 人 允许 使 用 完全 无 天 的 图 像 。 

我 们 知道 每 个 方程 都 党 噪声 影响 ， 所 以 要 把 这 里 的 位 姿 x 和 路 标 y 
看 成 服从 某 种 概率 分 布 的 随机 变量 ， 而 不 是 单独 的 一 个 数 。 因 此 ， 我 
们 关心 的 问题 就 变 成 : 当 我 拥有 某 些 运动 数据 U_ 和 观测 数据 z 时 ， 如 何 
来 确定 状态 量 xy 的 分 布 ? 进而 ， 如 果 得 到 了 新 时 刻 的 数据 ， 那 么 它们 
的 分 布 又 将 发 生 怎 样 的 变化 ? 在 比较 常见 且 合理 的 情况 下 ， 我 们 假设 状 
态 量 和 噪声 项 服从 高 斯 分 布 一 一 这 意味 着 在 程序 中 只 需要 储存 它们 的 均 
值 和 协 方差 矩阵 即 可 。 均 值 可 看 作 是 对 变量 最 优 值 的 估计 ， 而 协 方差 卸 
EURE SEN AMEE. AA, [a Ue AEA: 当 存 在 一 些 运 动 数据 
和 观测 数据 时 ， 我 们 如 何 去 佑 计 状 态 量 的 高 斯 分 布 ? 

我 们 依然 说 刁 处 地 地 扮 泗 一 下 小 克 卜 。 只 有 运动 方程 时 ， 相 当 于 我 
们 蒙 着 眼睛 在 一 个 未 知 的 地 方 走路 。 尽 管 我 们 知道 自己 每 一 步 走 了 多 
远 ， 但 是 随 着 时 间 增 长 ， 我 们 将 对 自己 的 位 置 越 来 越 不 确定 一 一 内 心 也 
束 越 不 安 。 这 说 明 在 输入 数据 受 噪 声 影 响 时 ， 我 们 对 位 置 方 兰 的 估计 将 
越 来 越 大 。 但 是 ， 当 我 们 具 开 眼睛 时 ， 由 于 能 够 不 断 地 观测 到 外 部 场 
景 ， 使 得 位 置 估计 的 不 确定 性 变 小 ， 我 们 惑 会 越 来 越 上 自信 。 如 果 用 椭圆 
或 李 球 直观 地 表达 协 方差 阵 ， 那 么 这 个 过 程 有 点 像 是 在 手机 地 图 软件 中 
走路 的 感觉 。 以 图 10-1 为 例 ， 读 者 可 以 想象 ， 当 没有 观测 数据 时 ， 这 个 
圆 会 随 着 运动 越 来 越 大 ;， 而 如 果 有 正确 的 观测 ， 圆 就 会 缩小 至 一 定 的 大 
小 ， 你 持 稳 定 。 








图 10-1 不 确定 性 的 直观 描述 。 左 侧 : 只 有 运动 方程 时 ， 由 于 下 一 时 刻 的 位 姿 是 在 上 一 时 
刻 基 础 上 添加 了 噪声 ， 所 以 不 确定 性 越 来 越 大 。 右 侧 : 存在 路 标点 时 ， 不 确定 性 会 明显 
减 小 。 不 过 请 注意 ， 这 只 是 一 个 直观 的 示意 图 ， 并 非 实 际 数 据 。 


上 面 的 过 程 以 比喻 的 形 却 解释 了 状态 估计 中 的 问题 ， 下 面 我 们 要 以 
定量 的 方式 来 看 得 它 。 在 第 6 讲 中 ， 我 们 介绍 了 最 大 似 然 估计， 所 到 把 
状态 估计 转换 为 最 小 二 乘 的 做 法 。 在 本 讲 我 们 要 更 仔细 地 讨论 这 个 问 
题 。 自 和 完 ， 由 于 位 姿 和 路 标 后 部 是 每 估计 的 变量 ， 我 们 改变 一 下 记号 ， 
4x, Ak 时 刻 的 所 有 未 知 量 。 它 包含 了 当前 时 刻 的 相机 位 姿 与 m 个 路 标 
扩 。 在 这 种 记号 的 意义 下 (里 然 与 之 前 和 有 不 同 ， 但 含义 古 清楚 的 ) ， 
写成 : 


人 
Lk = {ate Ui... lods (10.2) 


FIBI, JEK 时 刻 的 所 有 观测 记 作 z。 于 是 ， 运 动 方程 与 观测 方程 的 
形 却 可 与 得 更 加 简洁 。 这 里 不 会 出 现 y ， 但 我 们 心里 要 明日 这 时 x 中 已 


k=1,..., N. (10.3) 
Zk = h (£k) 十 UL 


| Le = f (Xy 1, Ue) + We 


PUES BK 时 刻 的 情况 。 我 们 希望 用 过 去 0 到 k 中 的 数据 来 估计 现 
在 的 状态 分 布 : 


P (Erto 81, 区 Th (10.4) 


下 标 0:k 表示 从 0 时 刻 a 到 k 时 刻 的 所 有 数据 。 请 注意 ，z 表示 所 有 在 kk 
时 刻 的 观测 数据 ， 它 可 能 不 止 一 个 ， 只 是 这 种 记 法 更 加 方便 。 


下 面 我 们 来 看 如 何 对 状态 进行 估计 。 按 照 贝 叶 斯 法 则 ， 把 z Sx, 交 
换 位 置 ， 有 : 
5 (10.5) 


读者 应 该 不 会 感到 陌生 。 这 里 第 一 项 称 为 似 然 “， 第 二 项 称 为 先 验 
， 似 然 由 观测 方程 给 定 ， 而 先 验 部 分 ， 我 们 要 明白 当前 状态 x 是 基于 过 
去 所 有 的 状态 估计 得 来 的 。 至 少 ， 它 会 受 %。, 影响 ， 于 是 按照 X。, HZ 
为 条 件 概率 展开 ; 


P (wi |£9, Uk; Z1:k-1) = | P (iini o, as ai) P (Erlo, tias zai) d 


(10.6) 


如 果 考 虑 更 久之 前 的 状态 ， 也 可 以 继续 对 此 式 进 行 展 开 ， 但 现在 我 
们 只 关心 k 时 刻 和 k- 1 时 刻 的 情况 。 至 此 ， 我 们 给 出 了 贝 叶 斯 估计 ， 虽 
然 上 式 还 没有 具体 的 概率 分 布 形 式 ， 所 以 还 没 法 实际 地 操作 它 。 对 这 一 
步 的 后 续 处 理 ， 方 法 上 产生 了 一 些 分 歧 。 大 体 上 讲 ， 存 在 若干 种 选择 : 
其 一 是 假设 马尔 可 夫 性 ， 简 单 的 一 阶 马 氏 性 认为 ，K 时 刻 状 态 只 与 k- 1 
时 刻 状 态 有 关 ， 而 与 再 之 前 的 无 关 。 如 果 做 出 这 样 的 假设 ， 我 们 就 会 得 
到 以 扩展 卡尔 曼 滤 波 ”EKF) 为 代表 的 滤波 器 方法 。 在 滤波 方法 中 ， 
我 们 会 从 某 时 刻 的 状态 估计 ， 推 导 到 下 一 个 时 刻 。 另 外 一 种 方法 是 依然 
考虑 k 时 刻 状 态 与 之 前 所 有 状态 的 关系 ， 此 时 将 得 到 非 线 性 优化 为 主 
体 的 优化 框架 。 非 线性 优化 的 基本 知识 已 在 前 文 介绍 过 。 目 前 视觉 
SLAM 的 主流 为 非 线 性 优化 方法 。 不 过 ， 为 了 让 本 书 更 全 面 ， 我 们 要 先 
介绍 一 下 卡尔 受 滤波 句 和 EKE 的 原理 。 


10.1.2 ”线性 系统 和 KF 
我 们 首先 来 看 滤波 器 模型 。 当 我 们 假设 了 马尔 可 夫 性 ， 从 数学 角度 


会 肥 生 哪些 变化 呢 ? 首 匈 ， 当 前 时 刻 状态 只 和 上 一 个 时 刻 有 天， 去 
(10.6) 中 右 侧 第 一 部 分 可 进一步 简化 : 


PG, if 3, Bo, Wn n-a) = F tl Eri Uh). (10.7) 
这 里 ， 由 于 k EZR k-12 WER, MARAR Ex, 和 


u 有 关 的 形式 ， 与 k 时 刻 的 运动 方程 对 应 。 第 二 部 分 可 从化 为 
P (x10, 和 in = P (3o, 1535-31, Zt —1)- (10.8) 


这 是 考虑 到 k 时 刻 的 输入 量 u, Sk 1 时 刻 的 状态 无 关 ， 所 以 我 们 把 u， 
拿 掉 。 可 以 看 到 ， 这 一 项 实际 上 是 k- ”1 时 刻 的 状态 分 布 。 于 是 ， 这 一 系 
列 方程 说 明 ， 我 们 实际 在 做 的 是 “如 何 把 所 1 时 刻 的 状态 分 布 推导 至 K 时 
刻 ” 这 样 一 件 事 。 也 就 是 说 ， 在 程序 运行 期 间 ， 我 们 只 要 维护 一 个 状态 


ee, WE AWE IAA ae By. dE, SROKA RM 
斯 分 布 ， 那 么 我 们 只 需 考虑 维护 状态 量 的 均值 和 协 方差 即 可 。 
我 们 从 形式 最 简单 的 线性 高 斯 系统 开始 ， 最 后 会 得 到 卡尔 曼 滤 波 
研 。 线 性 高 斯 系统 是 说 ， 运 动 方程 和 观测 方程 可 以 由 线性 方程 来 摘 述 : 
| Mp ARR RR Ht k=1,...,N. (10.9) 


Zk = Ck£k + Uk 


FERA ESTAS AAR Ps E A o WX ETRE RAE 
值 高 斯 分 布 : 


w, ~ N(0, R). ve ~ N(0,Q). (10.10) 
为 了 人 和 窗 洁 ， 笔 者 省 略 了 R 和 Q 的 下 标 。 现 在 ， 利 用 乌 尔 可 夫 性 ， 假 


设 我 们 知道 了 k- 1 时 刻 的 后 验 ( 在 k- 1 时 刻 看 来 ) 状态 估计 2x-1 及 其 协 
FAAP ， 现 在 要 根据 k 时 刻 的 输入 和 观测 数据 ， 确 定 X 的 后 验 分 布 。 
为 区 分 推导 中 的 先 验 和 后 验 ， 我 们 在 记号 上 做 一 点 区 别 ， 以 尖 帽 子 % 
KIRE, MRT RREI, WREDA 

卡尔 曼 滤 波 器 的 第 一 步 ， 通 过 运动 方程 确定 x 的 先 验 分 布 。 根 据 高 
斯 分 布 的 性 质 ， 显 然 有 : 


P (zk|20, U1:k, 21:k-1) = N (Arar + uk, Ap Pe AL + R) : (10.11) 

这 一 步 称 为 预测 ， 原 理 见 附录 A.3。 它 显示 了 如 何 从 上 一 个 时 刻 的 
状态 ， 根 据 输 入 信息 《但 是 有 噪声 ) 推断 当前 时 刻 的 状态 分 布 。 这 个 分 
AB tE te FEM o iu: 


Tk = Ap®p—1 + Up, P, ns A, P, AI + R. (10.12) 


AR BER. 259—718. FMT RITI Ar ER TRS F 
EVA HE EE AY DI Se : 


P (är Ee) = N (Cia, R). (10.13) 


为 了 得 到 后 验 概 雍 ， 我 们 想 要 计算 它们 的 乘积 ， 也 残 是 由 式 (10.5) 
给 出 的 贝 叶 斯 公式 。 然 而 ， 虽 然 我 们 知道 最 后 会 得 到 一 个 天 于 x， 的 局 其 


分 布 ， 但 计算 上 是 有 一 点 点 麻烦 的 ， 我 们 先 把 结果 设 为 x, ~N (x, , Pe), 
那么 : 
N(x, Py) = N (Crer, R) - N(zy, P). (10.14) 


1X ABTA WU TIS TT MEARI E ARE SEAM ARE E 
斯 分 布 ， 那 融 只 需 比 较 指 数 部 分 即 可 ， 而 无 顷 理会 遍 期 分 布 前 面 的 因子 
部 分 。 指 数 部 分 很 像 是 一 个 二 次 型 的 配方 ， 我 们 来 推导 一 下 。 自 和 完 把 指 
数 部 分 展开 ， ul 


(xp. = fr) P! (£k =- Ep) = (Zi = Cia) Ro (Zi n: C24) - (2 = zr) 石 -1 (£k = Ex) " 


(10.15) 


为 了 求 左 侧 的 Z* RUP. ， 我 们 把 两 边 展开 ， 并 比较 x 的 二 次 和 一 次 
系数 。 对 于 二 次 系数 ， 有 : 


Po! -CIR C.P, (10.16) 


该 式 给 出 了 协 方才 的 计算 过 程 。 为 了 便于 后 面 列 与 陈 子 ， 定 义 一 个 
中 间 变 量 : 


K=P,CiR. (10.17) 
根据 此 定义 ， 在 式 (10.16) 的 左右 各 乘 及 A: 
I=P,C{R'C,4+ ÊP ! = KC, 十 瑟瑟 -1 (10.18) 


FRA, 


^ 一 一 


P, = (I - KC) Px. (10.19) 
然后 册 比 较 一 次 项 的 系数 ， 有 : 
— 2I Pa, = —2zL R | Cuyo, — 231 Pay. (10.20) 
整理 ( 取 系 数 并 转 置 ) 得 : 
Pêr = CER 'zp 十 五 而/ (10.21) 

Pe DLP. 并 代入 式 (10.17)， 得 : 

tx = P,CIR zy, + PP, Ia, (10.22) 

= Kz, + (I— KCy4) & = Zp + K (zy — Cpt). (10.23) 


TEASE S ARHAR. ERES ERS gay 
以 归纳 为 “预测 ”(Predict〉 和 “更 新 ”(Update) 两 个 步骤 : 


1.913] : 
Ek = Åkĉk-1 tur, B= A&P& ALT +R. (10.24) 
2. 更 新 : IAK, CMA ROR S. 
K = P,C}(C,.P,Cl + Ry) `. (10.25) 


然后 计算 后 验 概率 的 分 布 。 


ke =T +K (Zk 一 CE.) 
P, = (I — KC) Px. 


Zee, RAET f ALY BR Bas BC as IE PSE SSE D. BR 
ZIEK A TAME TT ZU, RANEH EAE BE EE AC EN eA 
ZETA TINT. BUNGE, TEACHES, RAR EBC a TA Hà 
了 该 系统 中 的 最 大 后 验 概 率 佑 计 。 而 且 ， 由 于 高 期 分布 经 过 线性 变换 后 
仍 服从 高 期 分布 ， 所 以 整个 过 程 中 我 们 没有 进行 任何 的 近似 。 可 以 说 ， 
卡尔 受 滤波 融 构成 了 线性 系统 的 最 优 无 俩 们 计 。 


(10.26) 


10.1.3” 韭 线 性 系统 和 EKF 


在 理解 了 卡尔 曼 滤 波 之 后 ， 我 们 必须 要 深 清 一 点 : SLAM 中 的 运动 
方程 和 观测 方程 通 币 是 非 线性 函数 ， 尤 其 是 视觉 SLAM 中 的 相机 模型 ， 
需要 使 用 相机 内 参 模型 及 至 代数 表示 的 位 姿 ， 更 不 可 能 是 一 个 线性 系 
统 。 一 个 局 斯 分 布 ， 经 过 非 线 性 变换 后 ， 往 往 不 再 是 局 斯 分 布 ， 所 以 在 
非 线 性 系统 中 ， 我 们 必须 取 一 定 的 近似 ， 将 一 个 非 高 斯 分 布 近似 成 高 斯 
4] Al « 
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尔 曼 滤波 器 (Extended Kalman Filter, EKF) 。 通 常 的 做 法 是 ， 在 某 个 
点 附近 考虑 运动 方程 及 观测 方程 的 一 阶 泰 勒 展开 ， 只 保留 一 阶 项 ， 即 线 
性 的 部 分 ， 然 后 控 照 线性 系统 进行 推导 。 令 k- ”1 时 刻 的 均值 与 协 方 和 过滤 
EN êri Êi Æk 时 刻 ， 我 们 把 运动 方程 和 观测 方程 在 zi, 记 1 处 进行 
线性 化 〈 相 当 于 一 阶 泰勒 展开 ) ， 有 : 


Of 


OX 1 





Lk X f (£y 1, UK) + 





(Eyi 一 Ehi) + We. (10.27) 


Bh 


记 这 里 的 俩 寻 数 为 


Of 
F = 
Od 





(10.28) 





pi 


E, MMT Ee. IA: 


(x, — Êk) + Mx. (10.29) 





WIX E fid PBA 


Oh 


H = 一 一 
OZ 





(10.30) 


T k 


P (2 |a0, U1:k, Zo:k—-1) = N(f (&y a, ux) ,F Py F" + Ry). (10.31) 


X Ue HES AB OR SEIS ze T T AE EN HAERE, WAEA 
TI); Ze I E ELS 


ry = f (Ey 1,9); P, = FP.F! + R,. (10.32) 
然后 ， 考 虑 在 观测 中 我 们 有 : 
P (24: [as] 一 N(h (Tk) +H (Ek = Tk) ; Qk). (10.33) 


最 后 ， 根 据 最 开始 的 贝 叶 斯 展开 式 ， 可 以 推导 出 Xx。 的 后 验 概率 形 
式 。 我 们 上 略 去 中 间 的 推 寻 过 程 ， 只 介绍 其 结 来 。 读 者 可 以 仿照 卡尔 曼 滤 
波 右 的 方式 ， 推 性 EKF 的 预测 与 更 新 方程 。 简 而 谨 之 ， 我 们 会 完 定义 一 


个 卡尔 曼 增益 K, 
K, —-P,H'(HP,H' +Q) . (10.34) 
在 卡尔 受 增 益 的 基础 上 上， 后 验 概 率 的 形式 为 
T, = Xy Ky (zk — h (24,)) , P; = (I — KH) P,. (10.35) 


BR SIE ASH tS FER EZ JSR ASS ERI) A I BEE. TERK 
HEB SA eR, KIEA th Sot oath. WUTESLAM 
这 种 非 线性 的 情况 下 ， 生 给 出 了 单 次 线性 近似 下 的 最 大 后 验 佑 计 

(MAP) 。 


10.1.4 ”EKF 的 讨论 


EKF 以 形式 简洁 、 应 用 广泛 著称 。 当 想 要 在 某 段 时 间 内 估计 某 个 不 
确定 量 时 ， 我 们 首先 想到 的 就 是 EKF。 在 早期 的 SLAM 中 ，EKF 占 据 了 
很 长 一 段 时 间 的 主导 地 位 ， 研 究 者 们 讨论 了 各 种 各 样 的 滤波 器 在 SLAM 
中 的 应 用 ， 如 IF (信息 滤波 器 ) 9! . IKE! (Iterated KF) | UKF 


(Unscented KF) AIM FEU AIO SWF (Sliding Window 
Filter) (8 ， 等 等 中 ， 或 者 用 分 治 法 等 思路 改进 EKF 的 效率 9 中 。 直 
至 今日 ， 尺 省 我 们 认识 到 非 线 性 优化 比 滤波 右 占 有 明显 的 优势 ， 但 是 在 
计算 资源 爱 限 ， 或 符 估 计量 比较 简单 的 场合 ，EKF 仍 不 失 为 一 种 有 效 的 
s 

EKEF 有 哪些 局 限 呢 ? 

1. 和 首先， 滤波 亏 方 法 在 一 定 程 度 上 假说 了 马尔 可 夫 性 (DK 时 
刻 的 状态 只 与 

k- 1 ZAKS, mfg E k- 1 之 前 的 状态 和 观测 都 无 关 《〈 或 者 和 前 几 个 有 
限时 刻 的 状态 相关 ) 。 这 有 点 像 是 在 视 党 里 程 计 中 只 考虑 相 邻 两 师 关系 
一 样 。 如 果 当 前 帧 确实 与 很 久之 前 的 数据 有 关 《〈 例 如 回环 ) , WAVED 
器 就 会 难以 处 理 。 

而 非 线 性 优化 方法 则 倾 癌 于 使 用 所有 的 历史 数据 。 它 不 光 考 虑 邻近 
时 刻 的 特征 点 与 轨迹 关系 ， 更 会 把 很 久之 前 的 状态 也 考虑 进来 ， 称 为 全 
体 时 间 上 的 SLAM CFull-SLAMO 。 在 这 种 意义 下 ， 非 线性 优化 方法 使 
用 了 更 多 人 信息， 当然 也 需要 更 多 的 计算 。 

2 .与 第 6 讲 介 绍 的 优化 方法 相 比 ，EKF 滤 波 器 仅 在 X，， 处 做 了 一 次 
线性 化 ， 然 后 束 直 接 根据 这 次 线性 化 结果 ， 把 后 验 概率 给 算 了 出 来 。 这 
相当 于 在 说 ， 我 们 认为 该 点 处 的 线性 化 近似 在 后 验 概率 处 仍然 是 有 效 
的 ”。 而 实际 上 ， 妆 我 们 离开 工作 点 较 远 时 ， 一 阶 秦 勒 展开 并 不 一 定 能 
够 近似 整个 函数 ， 这 取决 于 运动 模型 和 观测 模型 的 非 线 性 情况 。 如 果 它 
们 有 强烈 的 非 线 性 ， 那 么 线性 近似 天 只 在 很 小 范围 内 成 立 ， 不 能 认为 在 
很 远 的 地 方 仍 能 用 线性 来 近似 。 这 束 是 EKFE 的 非 线 性 误 兰 ”， 也 是 它 的 
主要 问题 所 在 。 

在 优化 问题 中 ， 尽 管 我 们 也 做 一 阶 〈 最 速 下 降 ) BRYCE 
法 或 列 文 们 格 一 马 夸 尔 特 方法 ) 的 近似 ， 但 每 迭代 一 次 ， 状 态 估计 发 生 
改变 之 后 ， 我 们 会 章 新 对 新 的 估计 点 做 泰勒 展开 ， 而 不 人像 EKF 那 样 只 在 
国定 点 上 做 一 次 秦 靳 展开。 这 就 使 得 优化 方法 适用 汇 围 更 三 ， 在 状态 变 
化 较 大 时 亦 能 适用 。 

3. 从 程序 实现 上 来 说 ，EKEF 需 要 存储 状态 量 的 均值 和 方才 ， 并 对 它 
们 进行 维护 和 更 新 。 如 果 把 路 标 也 放 进 状态 ， 由 于 视觉 SLAM 中 路 标 数 


量 很 大 ， 这 个 存储 量 是 相当 可 观 的 ， 且 与 状态 量 呈 平方 增长 〈 因 为 要 存 
储 协 方差 矩阵 ) 。 因 此 ，EKF SLAM 被 普遍 认为 不 适用 于 大 型 场景 。 

由 于 EKF 存 在 这 些 明 显 的 缺点 ， 我 们 通 稼 认为 ， 在 同等 计算 量 的 情 
况 下 ， 非 线性 优化 能 取得 更 好 的 效果 HH 。 下 面 我 们 来 讨论 以 非 线性 优 
化 为 主 的 后 疹 。 我 们 将 主要 介绍 图 优化 ， 并 用 g2o 和 Ceres 体 验 一 下 后 问 
的 例子 。 


102 BA 与 图 优化 


如 果 你 做 过 视觉 三 维 香 建 ， 那 么 应 该 对 这 个 概念 再 熟悉 不 过 了 。 所 
请 的 Bundle Adjustment!  ， 是 指 从 视觉 重建 中 提炼 出 最 优 的 3D 模 型 和 相 
机 参数 〈 内 参数 和 外 参数 ) 。 从 每 一 个 特征 点 反射 出 来 的 几 束 光线 
(bundles of light rays) ， 在 我 们 把 相机 姿态 和 特征 点 空间 位 置 做 出 了 最 优 
的 调整 Cadjustment) 之 后 ， 最 后 收 束 到 相机 光 心 的 这 个 过 程 Pa TR 
为 BA。 


在 图 优化 框架 的 视觉 SLAM 算 法 里 ，BA 起 到 了 核心 作用 。 它 类 似 于 
求解 只 有 观测 方程 的 SLAM 问 题 。 在 最 近 几 年 视觉 SLAM 理 论 的 研究 
中 ，BA 算 法 不 仅 具 有 很 高 的 精度 ， 也 开始 具备 民 好 的 实时 性 ， 能 够 应 
用 于 在 线 计算 的 SLAM 场 景 中 。 而 在 21 世 纪 早 期 ， 虽 然 计 算 机 视觉 领域 
的 研究 者 们 已 经 开始 利用 BA 进行 午 构 ， 但 SLAM 的 研究 者 们 通常 认为 包 
伟大 量 特 征 点 和 相机 位 姿 的 BA 计算 量 过 大 ， 不 适合 实时 计算 。 和 直到 近 
十 年 ， 人 们 逐渐 认识 到 SLAM 问 题 中 BA 的 稀 朴 特性 ， 才 使 它 能 够 在 实时 
的 场景 中 应 用 外 。 因 此 ， 和 营 握 好 Bundle Adjustment， 深 入 研究 其 中 的 
理论 和 实践 细节 ， 是 做 好 视觉 SLAM 的 关键 。 


本 市 结合 第 5 讲 和 第 6 讲 的 内 容 ， 介 绍 Bundle Adjustment 的 模型 ， 并 
且 和 读者 逐步 探讨 求解 过 程 ， 特 别 是 探讨 它 的 黎 焉 性 ， 然 后 介绍 一 种 通 
用 的 快速 求解 方法 。 


10.2.1 投影 模型 和 BA 代价 函数 
在 第 5 讲 里 ， 我 们 曾 介 绍 了 投影 模型 和 畸变 ， 让 我 们 复习 一 下 整个 


投影 的 过 程 。 从 一 个 世界 坐标 系 中 的 点 p 出 及 ， 把 相机 的 内 外 参数 和 是 
变 部 考虑 进来 ， 最 后 投影 成 像 系 坐 标 ， 十 要 如 下 步 桑 。 


1. 自 完 ， 把 世界 坐标 转换 到 相机 坐标 ， 这 里 将 用 到 相机 外 参数 (R,t 


P' = Rp + t-[X',Y', ZT. (10.36) 


2. 然 后 ， 将 已 投 至 归 一 化 平面 ， 得 到 归 一 化 坐标 : 
P, = tata I" 2 [X'7Z', Y'/Z',1]*. (10.37) 
3. 考 虑 归 一 化 坐标 的 畸变 情况 ， 得 到 去 畸变 前 的 原始 像素 华 标 。 这 
里 暂时 只 竹 虑 径 回 畸变 : 


| ul, = uc (1 + kır? + ker?) 


(10.38) 
V, = vc (1 + kir? + korz) 
4. 了 最 后 ， 根 据 内 参 模型 ， 计 算 像 系 坐 标 : 
Us = frue + Cg | (10.39) 
Us = fyvc + Cy 


这 一 系列 计算 流程 看 似 有 些 复 末 。 我 们 用 流程 图 10-2 形 象 化 地 表示 
整个 过 程 ， 以 帮助 谈 者 理解 。 读 者 应 该 能 领会 到 ， 这 个 过 程 也 束 是 前 面 
讲 的 观测 方程 ， 之 前 我 们 把 它 抽象 地 记 成 : 


E = RL y). (10.40) 











P'=Rp+t =u, (Le kr? kr) 


P=[P./P,,P./P,,1] v, =v, (1+ kr? + krí) 





图 10-2 ”计算 流程 示意 图 。 左 侧 的 p EER PASER AWU y, 是 该 点 在 
图 像 平 面 上 的 最 终 像 素 坐 标 。 中 间 畸 变 模块 中 的 "* =" +00 。 


现在 ， 我 们 给 出 了 它 的 详细 参数 化 过 程 。 有 具体 地 说 ， 这 里 的 x 指 代 
此 时 相机 的 位 姿 ， 即 外 参 R,t ， 它 对 应 的 李 代 数 为 E 。 路 标 y 即 这 里 的 三 
维 点 p ， 而 观测 数据 则 是 像素 坐标 z^ =[u ,v. ]7 。 以 最 小 二 乘 的 角度 来 考 
E, MAW qd Ta 





e =z-h (é,p ). (10.41) 


然后 ， 把 其 他 时 刻 的 观 汕 量 也 考 碟 进来 ， 我 们 可 以 给 误 委 添加 一 个 
Bip. Bez, 为 在 位 姿 上 处 观察 路 标记 产生 的 数据 ， 那 么 整体 的 代价 函数 


(Cost Function) 7j 


sD D leyl = 3 3 D lley = h(&pj)ll". (10.42) 
对 这 个 最 小 二 乘 进行 求解 ， 相 当 于 对 位 姿 和 路 标 同时 做 了 调整 ， 也 
就 是 所 请 的 BA。 接 下 来 ， 我 们 会 根据 该 目标 函数 和 第 6 讲 介 绍 的 非 线 性 
优化 内 容 ， 逐 步 深 入 探讨 该 模型 的 求解 。 


10.2.2 BA 的 求解 


观察 在 上 一 市 中 的 观测 模型 nh (&,p )， 很 容易 判断 该 函数 不 是 线性 函 
数 。 所 以 我 们 希望 使 用 6.2 市 介绍 的 一 些 非 线性 优化 手段 来 优化 它 。 根 
扼 非 线性 优化 的 思想 ， 我 们 应 该 从 东 个 初始 值 开 始 ， 不 断 地 寻找 下 降 方 
问 Ax 来 找到 目标 函数 的 最 优 解 ， 即 不 断 地 求解 增 量 方程 (6.22) 中 的 增 量 
Ax 。 尽 官 误 关 项 者 是 针对 单个 位 姿 和 路 标点 的 ， 但 在 束 体 BA 目标 函数 
E, 我 们 必须 把 自 变量 定义 成 所 有 竺 优化 的 变量 : 


gres [Gig ants Bou D : (10.43) 


相应 地 ， 增 量 方 程 中 的 Ax ” 则 是 对 整体 目 变 量 的 增 量 。 在 这 个 意义 
当 我 们 给 目 变 量 一 个 增 量 时 ， 目 标 函 数 变 为 


= ;f(z + Aa] sIYY lei; + Fi; A€; + Ej ^p; . (10.44) 


Ae I 


EFE, 表示 整个 代价 函数 在 当前 状态 下 对 相机 姿态 的 俩 导数 ， 而 已 


表示 该 函数 对 路 标点 位 置 的 仿 寻 。 我 们 兽 在 7.7.3 市 中 介绍 了 它们 的 具体 
形式 ， 所 以 这 里 丈 不 再 展开 推 寻 了 。 现 在 ， 把 相机 位 姿 变 量 放 在 一 起 : 


Loe = |£1, &2 TI B €E Re”. (10.45) 


并 把 空间 点 的 变量 也 放 在 一 起 : 
= pi Da... Dal. eum (10.46) 


ABA, xX00.44)n] UAR poA WT F: 


1 if 
- | f(a + Ax)||? = 3 |e + FAz. + EA^z; | . (10.47) 


需要 注意 的 是 ， 该 式 从 一 个 由 很 多 个 小 型 二 次 项 之 和 ， 变 成 了 一 个 

更 整体 的 样子 。 这 里 的 雅 可 比 和 矩阵 下 RUF. 必须 是 整体 目标 函数 对 整体 变 

量 的 导数 ， 它 将 是 一 个 很 大 块 的 矩阵 ， 而 里 头 每 个 小 分 块 ， 需 要 由 每 个 

误差 项 的 导数 F， 和 巨 “ 拼 凌 "起 来 。 然 后 ， 无 论 我 们 使 用 高 斯 牛顿 法 还 
古 列 文 们 格 一 马 僵 尔 特 方法 ， 节 后 都 将 面 对 增 量 线性 方程 : 
Hia = 3. (10.48) 


根据 第 6 讲 的 知识 ， 我 们 知道 高 斯 和 牛顿 法 和 列 文 介 格 一 马 众 尔 特 方 
法 的 主要 差别 在 于 ， 这 里 的 互 是 取 JTJ 了 还 是 JTJ+AT 的 形式 。 由 于 我 们 提 
变量 归 类 成 了 位 姿 和 空间 氮 两 种 ， 所 以 雅 可 比 窍 阵 可 以 分 其 为 


J = [F E]. (10.49) 


那么 ， 以 高 斯 牛顿 法 为 例 ， 则 五 窍 阵 为 


(10.50) 


— a n 


E'F E'E 


TANE P SCA RSS OR ETI HP Bed] t RV RAEE. To 
友 现 ， 因 为 考虑 了 所 有 的 优化 变量 ， 这 个 线性 方程 的 维度 将 非 闸 大 ， 包 
含 了 所 有 的 相册 位 竣 和 路 标点 。 尤 其 是 在 视 交 SLAM 中 ， 一 幅 图 像 整 会 
提出 数 昌 个 特征 点 ， 大 大 增加 了 这 个 线性 方程 的 规模 。 如 条 直接 对 互 OK 
竟 来 计算 增 量 方程 ， 由 于 窍 阵 求 旬 是 复杂 度 为 O (n 3 JERE ， 这 是非 
HB Ce RA. SAIS ze, RIA 矩阵 是 有 一 定 的 特殊 结构 的 。 
利用 这 个 特殊 结构 ， 我 们 可 以 加 速 求解 过 程 。 


10.2.3 FARIEM ZM, 


211025 SESLAMIT] — RE ER XE VARI S ARERH WIERA., 
并 友 现 该 结构 可 以 目 然 、 显 式 地 用 图 优化 来 表示 3 中 。 本 市 将 评 细 讨论 
一 下 该 矩阵 稀 殉 结构 。 

HE BA i a PE X EH TE n] EES (x ) 引 起 的 。 考 虑 这 些 代价 函数 当 
中 的 其 中 一 个 ej 。 注 意 到 ， 这 个 误差 项 只 朱 述 了 在 5 看 到 p, 这 件 事 ， 只 


涉及 第 i 个 相机 位 姿 和 第 ) 个 路 标点 ， 对 其 余部 分 的 变量 的 导数 都 为 0。 
所 以 该 误差 项 对 应 的 雅 可 比 矩 阵 有 下 面 的 形式 ; 


OE; Qe;; 
dale) = (os. ...02x6; HE, 1026 ...02x3, ...0253, jp, 029: 2x2) . . (10.51) 
a J 


其 中 0, ,表示 维度 为 2x CHOKE, [HEBRO, , , HEM. HRA 
对 相机 姿态 的 偏 导 0e, /65 维度 为 2x 6， 对 路 标点 的 偏 导 6e, /Op, 维度 是 
2x 3。 这 个 误 弄 项 的 雅 可 比 沧 隆 ， 除 了 这 两 处 为 非 零 块 之 外 ， 其 余地 方 
部 为 零 。 这 体现 了 该 误 到 项 与 其 他 路 标 和 轨迹 无 天 的 特性 。 那 么 ， 它 对 
增 量 方程 有 何 影 响 ? 互 沧 阵 为 什么 会 产生 稀 玖 性 呢 ? 

以 图 10-3 为 例 ， 我 们 设 万 只 在 ij 处 有 非 零 其， 那么 它 对 五 的 页 献 为 
Tid > RAR EAEAN. ASS Si) 窍 阵 也 仅 有 4 个 非 零 块 ， 位 
PGi), Gj), Gi), Gi) 

对 于 整体 的 瑟 ， 由 于 : 


H = >》 JI Ji, (10.52) 
ij 


请 注意 i 在 所 有 相机 位 姿 中 取 值 ，j 在 所 有 路 标点 中 取 值 。 我 们 把 
进行 分 块 : 


H, H 
H= | — | | (10.53) 


J 
uiu 
| B00000000 
| 。 
J= DOROOORO H+ 228 7 WL 
T 1 | a 
| j | JOO8o0008o 
| PIU OU L 
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这 里 五 ， 只 和 相机 位 姿 有 关 ， 而 五 ,只 和 路 标点 有 关 。 当 我 们 过 故 守 
时 ， 以 下 事实 总 是 成 工 的 : 
LAGI BAZ, Ha MENHAR REH, 处 有 非 零 块 。 


2.[HER, H p Hex rah, REH AH HERREN. 


3. 对 于 五 ， 和 瑟 ，， 它 们 可 能 是 稀疏 的 ， 也 可 能 是 稠密 的 ， 视 具体 八 
观测 数据 而 定 。 
这 显示 了 HH 的 稀 琉 结构 。 之 后 对 线性 方程 的 求解 中 ， 也 正 需要 利用 


它 的 稀 芯 结构。 也 许 读 者 还 没有 很 好 地 领会 这 里 的 意思 ， 我 们 举 一 个 实 
例 来 直观 说 明 它 的 情况 。 假设 一 个 场景 内 有 2 个 相机 位 次 (C,,C,) 和 6 


个 路 标 (P,,P,,P,,P,,P-,P,) 。 这 些 相 机 和 点 云 所 对 应 的 变量 为 E ii 
Ap, j 71,7, 6。 相 机 C , 观测 到 路 标 P, ,P ,,P ;,P，， 相 机 C ,观测 到 路 标 
,P 。,P 。。 我 们 把 这 个 过 程 画 成 示意 图 10-4。 相 机 和 路 标 以 圆 形 节 点 表 
示 。 如 果 i 相机 能 够 观测 到 ij 点 云 ， 我 们 就 在 它们 对 应 的 节点 连 上 一 条 
边 。 

可 以 推出 ， 场 景 下 的 BA 目标 函数 应 该 是 


(Ilex)? + |lexell + llerall” + llerall” + |leaall” + llezall” + lleasll” + lezl?) 
(10.54) 


l 
2 





图 10-4 点 和 边 组 成 的 示意 图 。 该 图 显示 相机 C , 观测 到 了 路 标点 P,,P,,P,,P,， 相 机 C ,看 
本 有 | 有 
这 里 的 e; 使 用 之 前 定义 过 的 代价 函数 ， 即 式 (10.42)。 以 e n 为 例 ， 
EA S ÆC | 看 到 了 P , 这 件 事 ， 与 其 他 的 相机 位 次 和 路 标 无 关 。 令 Ji 
Jye ,, 所 对 应 的 雅 可 比 和 矩阵 ， 不 难看 出 e , 对 相机 变量 E， 和 路 标点 p sns 
的 俩 寻 都 为 0。 我 们 把 所 有 变量 以 x =(E 1 ,6 ,,p sp)! IER, Wa 





Oe11 oe Oe 


J11 = Dar 2 O2xa, 02x8, 02x3, 02x3, 02 ) : (10.55) 


———,0 ,一 一 一 
OE1 nin Opi 
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有 数值 ， 其 余 没 有 闫 色 的 区 域 表示 矩阵 在 该 处 数值 都 为 0。 那 么 上 和 面 的 J 
a 则 可 以 表示 成 图 10-5 所 示 图 案 。 同 理 ， 其 他 的 雅 可 比 沧 阵 也 会 有 类 似 


的 稀 玩 图 条。 


C1 (2 Py Ph P Py Ps Pg 


Jı =| [| 


图 10-5 Ja FARE RARER) mE. LET a AN ETA MY EN EE. LA LS 
数 维 数 比 点 云 参数 维 数 要 大 ， 所 以 C , 对 应 的 矩阵 其 要 比 P, XIM AIRE PEIR DE — 28. 





J T Fe BZ H hp eB BOT AE EERE, RATE REE, 按照 一 
XE MU VA als, AA RE ACHE YE AEA RE EA FE RE ED i TT Or 9 3 





图 10-6 所 示 。 
CC B, P, h P, P P 
hi [ | 
hz [ | L 
hs = LJ 
j= ^ = | m c 
Ja a LJ 
m £] E 
Jas Iz LJ 
J26 [| LJ 
10-6 JEn CEFR RE A RE CAL) 和 五 矩阵 的 稀疏 性 〈 右 ) , SAT RR oN BE EY 
应 的 矩阵 块 处 有 数值 ， 其 余 没 有 颜色 的 部 分 表示 和 矩阵 在 该 处 的 数值 始终 为 0。 
也 许 你 已 经 注意 到 了 ， 图 10-4 对 应 的 邻接 和 矩 隆 (Adjacency 


Matrix) ©! 和 上 图 中 的 五 FEM, BR SSAC DSTI ARR BEE 
一 任 的 结构 。 事 实 上 的 确 如 此 。 上 面 的 五 窍 阵 一 共有 8x 8 个 矩阵 块 ， 对 
TH 窍 阵 当中 处 于 非 对 角 线 的 矩阵 块 来 说 ， 如 来访 滤 阵 块 非 零 ， 则 其 位 
兽 所 对 应 的 变量 之 间 在 图 中 会 存在 一 条 边 ， 我 们 可 以 从 图 10-7 中 清晰 地 
看 到 这 一 点。 所 以 ， 互 距 阵 当中 的 非 对 角 部 分 的 非 零 矩阵 其 可 以 理解 为 
其 对 应 的 两 个 变量 之 间 存 在 联系 ， 或 者 可 以 称 之 为 约束 。 于 十， 我 们 友 
现 图 优化 结构 与 增 量 方程 的 稀 蚊 性 存在 看 明显 的 联系 。 


C, Py P; P4 Py Ps Pe 
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图 10-7 HERE PIERRE BRA EI CELERE IIR ZR. SAL EH RH US ZE CAR PER, 
表示 在 右 图 中 其 对 应 的 变量 C, 和 P ,之 间 存 在 一 条 边 e ,, o 





现在 考虑 更 一 般 的 情况 ， 假 如 我 们 有 m 个 相机 位 姿 ，n 个 路 标点。 
由 于 通 第 路 标的 数量 远 远 多 于 相机 ， 于 是 有 mn 福 m ”。 由 上 和 面 的 推理 可 
知 ， 实 际 的 五 矩阵 会 如 图 10-8 所 示 。 它 的 左上 角 块 显得 非 铝 小 ， 而 右 下 
角 的 对 角 块 占据 了 大 量 地 方 。 除 此 之 外 ， 非 对 角 部 分 则 分 布 着 散乱 的 观 
测 数 据 。 由 于 它 的 形状 很 价 和 迎头 ， 叉 称 为 祷 头 形 (Arrow-like〉 和 矩阵 中 
。 同 时 ， 它 也 很 像 一 把 锅 于 ， 所 以 笔者 也 称 其 为 锅 形 窍 阵 。 
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图 10-8 ”一 般 情况 下 的 五 窍 阵 。 


对 于 具有 这 种 黎 焉 结构 的 五 ， 线 性 方程 了 H Ax =g 的 求解 会 有 什么 不 
同 呢 ?现实 当中 存在 着 奢 干 种 利用 五 PER RDE, ANP 
绍 视觉 SLAM 里 一 种 最 常用 的 手段 ，Schur 消 元 。 在 SLAM 研 究 中 亦 称 为 
Marginalization (边缘 化 )。 


仔细 观察 一 下 图 10-8， 我 们 不 难 发 现 这 个 矩阵 可 以 分 成 4 个 块 ， 和 
式 (10.53) 一 人 改 。 左 上 和 角 为 对 角 块 看 了 泗 ， 每 个 对 角 块 元 标的 维度 与 相机 位 
次 的 维度 相同 ， 且 是 一 个 对 角 块 看 阵 。 右 下 角 也 是 对 角 块 矩阵 ， 每 个 对 
角 块 的 维度 是 路 标的 维度 。 非 对 角 块 的 结构 与 基体 观测 数据 相关 。 我 们 
首先 将 这 个 矩阵 按照 图 10-9 所 示 的 方式 做 区 域 划 分 ， 谈 者 不 难 发 现 ， 这 
4 个 区 域 正 好 对 应 了 公式 (10.50) 中 的 4 个 矩阵 块 。 为 了 后 续 分 析 方 便 ， 我 


们 记 这 4 个 块 为 B,E,ET,C 。 
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图 10-9 H ERBERK ERRI o 
Fæ, WARRENA UHH Ax 2g ENU FÉR: 


B E 
E! C 


Aw, 
Ay 


U 
一 | | ; (10.56) 
wW 


其 中 B 是 对 角 块 矩阵 ， 每 个 对 角 块 的 维度 和 相机 参数 的 维度 相同 ， 
对 和 角 块 的 个 数 是 相机 变量 的 个 数 。 由 于 路 标 数量 会 远 远 大 于 相机 变量 个 
数 ， 所 以 C 往往 也 远大 于 B 。 三 维 空间 中 每 个 路 标 扣 为 三 维 ， 于 是 C ÓB 
阵 为 对 角 块 窍 阵 ， 每 个 块 为 3x 3 滤 阵 。 对 角 块 窍 阵 求 涵 的 难度 远 小 于 对 
一 般 窍 阵 的 求 涵 难度 ， 因 为 我 们 只 需要 对 那些 对 角 线 矩阵 块 分 列 求 逆 即 
可 。 考 虑 到 这 个 特性 ， 我 们 对 线性 方程 组 进行 高 斯 消 元 ， 目 标 是 消去 右 
上 角 的 非 对 角 部 分 已 ft: 
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整理 ， 得 : 
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经 过 消 元 之 后 ， 方 程 组 第 一 行 变 成 和 Ax， 无 天 的 项 。 单 独 把 它 拿 
来 ， 得 到 关于 位 姿 部 分 的 增 量 方程 : 

[B — EC !E'|Azx.—v- EC tw. (10.59) 

这 个 线性 方程 的 维度 和 B 和 是 阵 一 样 。 我 们 的 做 法 是 先 求 解 这 个 方 
程 ， 然 后 把 解 得 的 Ax。 代入 到 原 方程 ， 然 后 求解 Ax， 。 这 个 过 程 称 为 
Marginalization*?) ， 或 者 Schur 消 元 (Schur Elimination) 。 相 比 于 直接 
解 线 性 方程 的 做 法 ， 它 的 优势 在 于 : 

1. 在 消 元 过 程 中 ， 由 于 C 为 对 角 块 ， 所 以 C-! 容易 解 出 。 

2. 求 解 了 了 Ax 之后， 路标 部 分 的 增 量 方程 由 Ax, =C ! (w-E T Ax, ) 给 
出 。 这 依然 用 到 了 Ci! 易于 求解 的 特性 。 


于 是 ， 边 缘 化 的 主要 计算 量 在 于 求解 陈 (10.59)。 关 于 这 个 方程 ， 我 
们 能 说 的 就 不 多 了 。 它 仅 是 一 个 普通 的 线性 方程 ， 没 有 特殊 的 结构 可 以 
利用 。 我 们 将 此 方程 的 系数 记 为 S$ ， 它 的 稀 焉 性 如 何 呢 ? 图 10-10 显 示 了 
进行 Schur 消 元 之 后 的 一 个 S 实例 ， 可 以 看 到 它 的 稀 玻 性 古人 不 规则 的 。 





mc 
图 10-10 “对 五 矩阵 进行 Schur 消 元 后 S E BERERA. 


AWE H 矩阵 的 非 对 角 块 处 的 非 零 元 系 对 应 着 相机 和 路 标的 关 
联 。 那 么 ， 进 行 了 Schur 消 元 后 S 的 稀 焉 性 是 否 具 有 物理 意义 呢 ? 答案 是 
肯定 的 。 此 处 我 们 不 加 证 明 地 说 ，S 和 矩阵 的 非 对 角 线 上 的 非 零 矩阵 块 ， 
表示 了 该 处 对 应 的 两 个 相机 变量 之 间 存 在 着 共同 观测 的 路 标点 ， 有 时候 
称 为 共 视 〈Co-visibility) 。 反 之 ， 如 果 访 块 为 零 ， 则 表示 这 两 个 相机 没 
有 共同 观测 。 例 如 图 10-11 所 示 的 稀疏 和 矩阵， 左上 角 前 4x ”4 个 矩阵 块 可 
以 表示 对 应 的 相机 变量 C, ,C , ,C ,,€C ,之 间 有 共同 观测 。 





图 10-11 DAS 矩阵 中 前 4x ANAE RER JA, 30-7 S DS PFE REERS WS, 不 为 零 ， 表 示 相 
NLC ,和 相机 C，,、C , 之 间 有 共同 观测 点 ; 而 $ ,, 为 零 则 表示 C , MIC, 之 间 没 有 共同 观测 的 路 
标 。 


于 是 ，S 矩阵 的 稀 玖 性 结构 当 取 决 于 实际 观测 的 结果 ， 我 们 无 法 提 
前 预知 。 在 实践 当中 ， 例 如 ORB-SLAMr3I 中 的 Local Mapping 环 节 ， 在 
做 BA 的 时 候 刻 意 选 择 那 些 具 有 共同 观测 的 帧 作为 关键 帧 ， 在 这 种 情况 
下 Schur 消 元 后 得 到 的 S Wie Ha FE BE AN, FR SRE AN ee SN 
执行 ， 所 以 这 种 做 法 也 是 可 以 接 党 的 。 但 是 在 另 一 些 方 法 里 面 ， 例 如 
DSOP? , OKVISU* 等 ， 它 们 采用 了 滑动 窗口 方法 (Sliding Window) 。 
这 类 方法 对 每 一 帧 都 要 求 做 一 次 BA 来 防止 误差 的 累积 ， 因 此 它们 也 必 
须 采 用 一 些 技巧 来 你 持 S 和 矩阵 的 稀 焉 性 。 旋 者 如 采 硕 望 能 够 更 加 深入 这 
一 块 ， 可 以 参考 它们 的 论文 。 这 里 就 不 谈 这 些 过 于 细节 的 事情 了 。 

从 概率 角度 来 看 ， 我 们 称 这 一 步 为 边缘 化 ， 是 因为 我 们 实际 上 把 求 
(Ax, , Ax, ) 的 问题 ， 转 化 成 了 先 求 Ax。 ， 再 求 Ax, 的 过 程 。 这 一 步 相 当 于 
fil T AE TERES EDT: 


PUG, Bo} = d) PES RU. (10.60) 


结果 是 求 出 了 关于 x. 的 边缘 分 布 ， 故 称 边缘 化 。 在 前 面 讲 的 边缘 化 
过 程 中 ， 我 们 实际 把 所 有 的 路 标点 都 给 边缘 化 了 。 根 据 实 际 情况 ， 我 们 
也 能 选择 一 部 分 进行 边缘 化 。 同 时 ，Schur 消 元 只 是 实现 边缘 化 的 其 中 
一 种 方式 ， 同 样 可 以 使 用 Cholesky 分 解 进 行 边缘 化 。 

读者 可 能 会 继续 问 ， 在 进行 了 Schur 消 元 后 ， 我 们 还 需要 求解 线性 
方程 组 (10.59)。 对 它 的 求解 是 否 还 有 什么 技巧 呢 ? 很 遗憾 ， 这 部 分 束 属 
于 传统 的 窃 阵 数值 求解 了 ， 通 党 是 用 分 解 来 计算 的 。 不 管 采 用 哪 种 求解 
办 法 ， 我 们 都 建议 利用 五 ”的 稀 朴 性 进行 Schur 消 元 。 不 光 是 因为 这 样 可 
以 提高 速度 ， 也 因为 消 元 后 的 9 和 矩阵 的 条 件数 往往 比 之 前 的 五 窍 阵 要 
小 。Schur 消 元 也 并 不 意味 着 将 所 有 路 标 消 元 ， 将 相机 变量 消 元 也 是 
SLAM 当 中 采用 的 手段 。 


10.2.4 &mfSETZ ER BW 


在 前 面 的 BA 问题 中 ， 我 们 最 小 化 误差 项 的 二 范 数 平方 和 作为 目标 
图 数 。 这 种 做 法 虽然 很 百 观 ， 但 存在 一 个 严重 的 问题 : 如 打出 于 误 匹 配 
等 原因 ， 菏 个 误差 项 给 的 数据 是 错误 的 ， 会 友 生 什么 呢 ? 我 们 把 一 条 原 
本 不 应 该 加 到 图 中 的 边 给 加 进去 了 ， 然 而 优化 算法 并 不 能 辨 列 出 这 和 是个 
错误 数据 ， 它 会 把 所 有 的 数据 都 当 作 误差 来 处 理 。 这 时 ， 算 法 会 看 到 一 
条 误 帮 很 大 的 边 ， 它 的 标 度 也 很 大 ， 音 味 痢 调整 与 它 相 关 的 变量 会 使 目 
标 函 数 下 降 更 多 。 所 以 ， 算 法 将 试图 调整 这 条 边 所 连接 的 市 点 的 合计 
值 ， 使 它们 顺应 这 条 边 的 无 理 要 求 。 由 于 这 条 边 的 误差 真 的 很 大 ， 往 往 
会 抹 平 了 其 他 正确 边 的 有 影响， 使 优化 算法 专注 于 调整 一 个 错误 的 值 。 这 
显然 不 是 我 们 希望 看 到 的 。 


出 现 这 种 问题 的 原因 是 ， 当 误 天 很 大 时 ， 二 苑 数 增长 得 太 快 。 于 是 
束 有 了 核 了 水 数 的 存在 。 核 函数 你 证 每 条 边 的 误 帮 不 会 大 得 没 边 而 掩 新 挥 
其 他 的 边 。 有 基体 的 方式 是 ， 把 原先 误 乍 的 二 范 数 度量 答 换 成 一 个 增长 没 
有 那么 快 的 函数 ， 同 时 保证 目 己 的 光滑 性 质 〈 不 然 无 法 求 寻 ! ) 。 因 为 
它们 使 得 整个 优化 结果 更 为 稳健 ， 所 以 义 叫 它们 息 棒 核 函 数 (Robust 
Kernel) 。 


绰 棒 核 函 数 有 许多 种 ， 例 如 最 香 用 的 Huber 核 : 


l 9 we z Ô, 
H (e) = 2 le (10.61) 
0 (lel 一 50) 其 他 


RIES, RAe KTNA 后 ， 函 数 增长 由 二 次 形 却 变 成 
TREA, HATRA RIRA. HEF, Hubert R% OA É 
滑 的 ， 可 以 很 方便 地 求 导 。 网 10-12 显 示 了 Huber 核 函数 与 二 次 函数 的 对 
比 ， 可 见 在 误 兰 较 大 时 Huber 核 轴 数 增长 明显 低 于 二 次 函数 。 





图 10-12 Hubert% HR. 


除了 Huber 核 之 外 ， 还 有 Cauchy 核 、Tukey 核 ， 等 等 ， 恋 者 可 以 看 看 
g20 和 Ceres 部 提供 了 哪些 核 疯 数 。 


10.2.5 4 


本 我 们 重点 介绍 了 BA 中 的 黎 焉 性 问题 。 不 过 ， 实 践 当 中 ， 有 许 
多 软件 库 为 我 们 提供 了 细节 操作 ， 而 我 们 需要 做 的 主要 是 构造 Bundle 
Adjustment 问 题 ， 设 置 Schur 消 元 ， 然 后 调用 笛 密 或 者 黎 焉 矩阵 求解 器 对 
变量 进行 优化 即 可 。 如 果 读 者 希望 更 深入 地 了 解 BA， 可 以 在 阅读 完 本 
市 的 基础 上 ， 进 一 步 参 考 文献 [26] 学 习 。 

下 面 的 两 节 ， 我 们 将 使 用 g20o 和 Ceres 两 个 库 来 做 Bundle 
Adjustment。 为 了 体现 出 它们 的 区 列 ， 我 们 会 让 这 个 程序 共用 很 多 代 
人 码 。 共 用 的 代码 主要 在 common 文 件 夹 下 ， 其 他 的 文件 则 不 同 。 我 们 将 
使 用 公开 的 数据 库 [75] 当 中 的 文件 来 进行 编程 练习 。 


10.3 KEk: g2o 


10.3.1 BA 数据 集 


在 本 讲 代码 目录 下 的 common 文 件 夹 是 两 个 实验 共有 的 数据 集 部 
分 。 它 的 布局 如 图 10-13 所 示 。 其 中 ， fl ags 文 件 严 下 的 两 个 文件 定义 了 
CommandArgs 这 个 类 ， 该 类 用 来 解析 用 户 输入 的 参数 ， 同 时 也 对 程序 需 
要 的 参数 提供 默认 值 及 文档 说 明 。BundleParams 这 个 类 定义 了 Bundle 
Adjustment 使 用 的 所 有 参数 ， 也 调用 了 CommandArgs 关 型 的 变量 。 由 于 
CommandArgs 这 个 类 的 存在 ， 我 们 可 以 直接 在 程序 后 和 面 使 用 -help 来 但 看 
程序 所 有 参数 的 人 台 义 ， 使 用 方式 可 以 参考 议程 序 中 BundleParams 基 型 的 

COMMON 
4 flags 
command_args.cpp 
command_args.h 
4 tools 
random.h 
rotation.h 
BALProblem.cpp 
BALProblem.h 


BundleParams.h 


projection.h 





图 10-13 common 文件 夹 目录 结构 。 


tools 是 一 些 数 学 工具 畏 效 ， 该 痢 可 目 行 租 看 。 我 们 的 相机 和 路 标 参 
数 与 前 面 提 到 的 代价 函数 ， 即 式 (10.42) 保 持 一 致 ， 不 过 在 实验 中 我 们 要 


把 相机 内 参 也 当 作 优化 变量 考虑。 我 们 用 fk ,k , 来 表示 焦距 和 晴朗 ，37 


参数 表示 平移 ，3 个 参数 表示 旋转 〈 于 是 相机 一 共有 9 个 未 知 量 ) ， 路 标 
也 使 用 三 维 坐 标 来 表示 。projection.h 是 前 面 提 到 的 投影 计算 流程 图 10-2 
的 代码 实现 。 由 于 g20 和 ceres 都 会 用 到 这 个 投影 模型 ， 因 此 这 个 文件 也 
将 被 两 个 工程 所 共 宇 。 

除 此 之 外 ，BALProblem 这 个 类 将 保存 我 们 的 BundleAdjustment 所 需 
要 的 所 有 数据 ， 其 中 束 包 括 了 相机 和 跤 标 之 间 的 关联 ， 以 及 相机 变量 和 
路 标 变 量 的 初始 值 ， 这 些 数 据 的 原始 信息 者 保存 在 data 文 件 夹 下 的 TXT 
文件 当中 。 我 们 可 以 打开 data/problem-16-22106-pre.txt 查 看 文件 信息 : 


1 |16 22106 83718 
-3.859900e+02 3.871200e+02 


-3.844000e+01 4.921200e+02 
-6.679200e*02 1.231100e+02 
-5.991800e+02 4.079300e+02 





它 的 第 一 行 记 录 了 相机 数量 、 路 标 数 量 和 总 的 观测 数量 。 从 第 二 行 
起 ， 分 别 记录 第 i 个 相机 观测 第 i 个 路 标 所 看 到 的 像素 坐标 。 这 束 构 成 了 
一 个 BA 问题 ， 尔 可 看 成 由 纯 观 测 方程 组 成 的 SLAM 问 题 。 除 此 之 外 ， 
BALProblem 也 提供 了 将 数据 导出 到 PLY 文 件 这 样 的 功能 ， 方 便 用 户 通 
过 Meshlab 软 件 但 看 点 云 的 三 维 信 息 。 


10.3.2”g20 求 解 BA 


下 面 来 考虑 如 何 使 用 g2o 求 解 这 个 BA 问题 。 和 以 前 一 样 ，g2o 使 用 
图 模型 来 摘 述 问题 的 结构 ， 所 以 我 们 要 用 节点 来 表示 相机 和 路 标 ， 然 后 
用 边 来 表示 它们 之 间 的 观测 。 我 们 仍然 使 用 目 定 义 的 点 和 边 ， 只 需 上 履 兰 
一 些 关 键 函 数 即 可 。 针 对 相机 和 路 标 ， 我 们 可 以 定义 如 下 继承 ， 使 用 
override KETRIN EK E RA i: 


四 slambook/ch10/ g20 custombundle/g20o bal class.h 


#include "g2o/core/base vertex.h" 


#include "g20o/core/base binary edge.h" 


#include <Eigen/Core> 


#include "ceres/autodiff.h" 


#include "tools/rotation.h" 


#include "common/projection.h" 


// 节点 没什么 好 说 的 ， 直 接 把 更 新 量 看 作 向 量 求 加 法 即 可 
class VertexCameraBAL : public g20o::BaseVertex«9,Eigen::VectorXd» 
1 
public: 
EIGEN MAKE ALIGNED OPERATOR NEW; 
VertexCameraBAL () {} 
virtual bool read(std::istream& is) {return false;} 
virtual bool write ( std::ostream& os ) const {return false;} 


virtual void setToOriginImpl () {} 


virtual void oplusImpl(const double* update) override { 
Eigen: :VectorXd::ConstMapType v(update, VertexCameraBAL: :Dimension) ; 


estimate += v; 


a |F 


2 | class VertexPointBAL : public g2o::BaseVertex<3, Eigen: :Vector3d> 


$$ x 

28 | public: 

29 EIGEN MAKE ALIGNED OPERATOR NEW; 

30 VertexPointBAL () {} 

31 virtual bool read( std::istream& is) {return false;} 

32 virtual bool write(std::ostream& os) const {return false;} 
33 virtual void setToOriginImpl() {} 

34 virtual void oplusImpl(const double* update) override 1 
35 Eigen::Vector3d::ConstMapType v(update); 

36 estimate += v; 

37 } 





EM SHRI, Wee RANT A ZIKA. BR 
都 对 应 了 一 个 代价 函数 (10.42)， 同 时 为 了 避免 复杂 的 求 导 运算 ， 我 们 借 
助 Ceres 库 当中 的 Autodi 任 〈 即 目 动 求 导 ) Dike. DRE m BRA eZ 
求 寻 的 公式 实现 在 括号 运算 从 operator() 里 ， 代 人 码 如 下 : 


kb) slambook/ch10/ g20 custombundle/g20o bal class.h 


class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, 
VertexCameraBAL, VertexPointBAL>{ 
public: 

EIGEN MAKE ALIGNED OPERATOR NEW; 

EdgeObservationBAL () {} 


virtual bool read(std::istream& /*is*/){ return false;} 
virtual bool write(std::ostream& /*os*/)const { return false;} 


virtual void computeError() override // RARE HA, 使 用 operator() 计 算 代 价 
函数 


1 
const VertexCameraBAL* cam = static cast«const VertexCameraBAL*»(vertex(0)) 
const VertexPointBAL* point = static cast«const VertexPointBAL*>(vertex(1) ) 
(xthis) (cam->estimate().data(), point->estimate().data(),  error.data()); 

} 


// 为 了 使 用 Ceres 求 导 功能 而 定义 的 函数 ， 让 本 类 成 为 拟 函 数 类 
template<typename T> 


32 


33 


36 


37 


bool operator()( const T* camera, const T* point, T* residuals )const 


{ 


T predictions [2]; 


CamProjectionWithDistortion(camera, point, predictions) ; 


residuals[0] = predictions[0] - T(measurement()(0)); 
residuals[1] = predictions[1] - T(measurement () (1) ) ; 


return true; 


virtual void linearizeOplus() override 


{ 


const VertexCameraBAL* cam = static cast«const VertexCameraBAL*>(vertex (0) ) 
const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1)) 
typedef ceres::internal::AutoDiff«EdgeÜbservationBAL, double, 


VertexCameraBAL: :Dimension, VertexPointBAL::Dimension> BalAutoDiff; 


Eigen: :Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen:: 
RowMajor> dError_dCamera; 

Eigen: :Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen: :RowMajor 
> dError_dPoint; 

double *parameters[] = { const_cast<double*>(cam->estimate().data()), 
const_cast<double*>(point->estimate().data()) }; 

double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() }; 
double value [Dimension] ; 

// Ceres 中 的 自动 求 导 函数 用 法 ， 需 要 提供 operator() 函数 成 员 

bool diffState = BalAutoDiff::Differentiate(*this, parameters, Dimension, 


value, jacobians); 


// copy over the Jacobians (convert row-major -> column-major) 
if (diffState) ( 


.jacobianÜplusXi = dError dCamera; 


.jacobianÜplusXj = dError dPoint; 

) else ( 
assert (0 && "Error while differentiating") ; 
.jacobianOplusXi.setZero(); 


.jacobianOplusXi.setZero(); 


以 上 定义 了 本 问题 中 使 用 的 和 点 和 边 。 下 面 需要 根据 BALProblem 
关 当 中 的 实际 数据 来 生成 一 些 节 点 和 边 ， 交 给 g2o 进 行 优化 。 值 得 注意 
的 是 ， 为 了 元 分 利用 BA 中 的 稀 政 性， 需要 在 这 里 将 路 标 中 的 
setMarginalized 属 性 设置 为 tue。 代 码 的 主要 片段 如 下 : 


中 slambook/ch10/g20 custombundle/g20 bundle.cpp (CHE) 


void BuildProblem(const BALProblem* bal problem, g20o::SparseÜptimizer* optimizer, 


const BundleParams& params) 


{ 


const int num points = bal_problem->num_points() ; 
const int num cameras = bal, problem-»num cameras(); 
const int camera block size = bal, problem-»camera, block size(); 


const int point block size = bal problem-»point block size(); 


// 添加 相机 和 对 应 的 初始 值 
const double* raw cameras = bal_problem->cameras() ; 


for(int i = 0; i < num cameras; ++i) 


{ 
ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i, 
camera block size); 
VertexCameraBAL* pCamera = new VertexCameraBAL(); 
pCamera->setEstimate(temVecCamera) ; // 初始 值 
pCamera->setId(i) ; 
optimizer-»addVertex(pCamera); 

} 


// Set point vertex with initial value in the dataset. 
const double* raw points = bal problem-»points(); 
// const int point block size = bal problem-»point block size(; 
for(int j = 0; j < num points; ++j) 
1 
ConstVectorRef temVecPoint(raw points + point block size * j, 
point block size); 
VertexPointBAL* pPoint - new VertexPointBAL(); 
pPoint->setEstimate(temVecPoint) ; // 点 云 的 初始 值 
pPoint->setId(j + num cameras); // 为 了 不 和 相机 冲突 ， 加 上 相机 的 4d 


// 设置 该 点 在 解 方程 时 进行 Schur 消 元 
PPoint->setMarginalized(true) ; 


optimizer->addVertex(pPoint) ; 


// 为 图 添加 边 
const int num observations = bal_problem->num_observations(); 


const double* observations = bal, problem-»observations(); 


for(int i = 0; i < num observations; ++i) 


{ 
EdgeObservationBAL* bal edge = new EdgeObservationBAL() ; 


const int camera id = bal_problem->camera_index()[i]; // 使 用 前 面 的 id 获取 


相机 


42 const int point id = bal_problem->point_index() [i] + num cameras; // 使 用 前 
面 的 id 获得 点 

43 // Huber loss 函数 (默认 为 不 用 设置 ) 

44 if (params.robustify) 

45 { 

46 g20::RobustKernelHuber* rk = new g20::RobustKernelHuber; 

47 rk->setDelta(1.0); 

48 bal_edge->setRobustKernel (rk); 

49 } 

50 // REA PRSE BE S IN OS 

51 bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex ( 


camera_id))); 
52 bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex ( 


point, id))); 


53 // 设置 其 协 方差 矩阵 〈 在 此 处 为 单位 矩阵 ) 

54 bal_edge->setInformation (Eigen: :Matrix2d::Identity()); 

55 // 设置 边 所 对 应 的 观测 值 

56 bal_edge->setMeasurement (Eigen::Vector2d(observations[2*i*0],observations 
[2*i + 1])); 

57 

58 optimizer->addEdge(bal_edge) ; 

59 } 

e |} 


10.3.3 求解 


在 使 用 g2o 时 ， 求 解 的 设置 无 非 这 几 种 : 1. 使 用 何 种 方法 《〈“ 列 文 介 
格 一 马 俯 尔 特 方法 、DogLeg 等 ) 来 定义 非 线 性 优化 的 下 降 末 上 略 ; 2. 使 用 
哪 类 线性 求解 硕 。 注 音 到 这 里 需要 使 用 人 到 稀 玩 性 ， 所 以 必须 选用 黎 下 的 
AXE ds 

由 slambook/ch10/g20_custombundle/g20_bundle.cpp (F Ex) 


t 





ypedef g2o::BlockSolver<g2o: :BlockSolverTraits<9,3> > BalBlockSolver; 


void SetSolverÜptionsFromFlags (BALProblem* bal problem, const BundleParams& params, 


{ 


g20::SparseOptimizer* optimizer) 


BalBlockSolver* solver_ptr; 


g20::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0; 


// 使 用 稠密 计算 方法 


if (params.linear_solver == "dense_schur" ) { 


11 linearSolver = new g20::LinearSolverDense«BalBlockSolver::PoseMatrixType»() 


* 
) 


12 } 

13 // AE MA te i XA 

14 else if (params.linear solver == "sparse schur") { 

15 linearSolver = new g20::LinearSolverCholmod«BalBlockSolver::PoseMatrixType 
>(); 

16 // 让 solver X48 BE ut 4p AE JT Rae Fp LE 

17 dynamic, cast«g20::LinearSolverCholmod«BalBlockSolver::PoseMatrixType»* >( 


linearSolver) ->setBlockOrdering (true) ; 


18 } 

19 

20 solver_ptr = new BalBlockSolver(linearSolver) ; 

21 

22 g20::OptimizationAlgorithmWithHessian* solver; 

23 if(params.trust region strategy == "levenberg_marquardt") { 

24 solver = new g20::OptimizationAlgorithmLevenberg(solver_ptr); // 使 用 列 文 
l He — By 5 IET EE 

25 } 

26 else if(params.trust_region_strategy == "dogleg"){ 

27 solver = new g2o::ÜptimizationAlgorithmDogleg(solver ptr); // 使 用 DogLeg 
F k 

28 } 

29 else 

30 { 

31 std::cout << "Please check your trust region strategy parameter again.."<< 
std::endl; 

32 exit(EXIT. FAILURE) ; 

33 } 

34 

35 optimizer->setAlgorithm(solver) ; 

x |} 


现在 问题 基本 上 挫 建 好 了 。 我 们 通过 BuildProblem 函 数 完 成 了 对 有 目 
标 函 数 的 构造 ， 也 通过 SetSolverOptionsFromFlags 函 数 使 用 用 户 的 输入 
参数 来 设置 优化 求解 。 和 狮 下 要 做 的 束 古 思考 该 如 何 求解 这 个 问题 了 。 有 
了 g20 这 样 的 优化 库 ， 求 解 基 本 上 可 以 一 步 完 成 ， 这 点 跟前 面 介绍 的 g20 
FAYE J LAR XE — REIS: 


ic) slambook/ch10/g20_custombundle/g20_bundle.cpp C Fr Ex) 


1 | optimizer.initializeOptimization() ; 


2 | optimizer .setVerbose (true); 





3 | optimizer.optimize(params.num iterations); 


使 用 Meshlab 分 别 打开 g2o 执 行文 件 严 下 的 initial.ply 和 fi nal.ply 两 个 
文件 ， 优 化 前 后 的 效果 如 图 10-14 所 示 。 





初始 但 优化 值 
110-14 ”g20 优 化 结果 。 左 侧 为 优化 前 急 始 值 ， 石 侧 为 优化 后 的 。 


由 于 g2o 库 本 里 公开 API 说 明 也 少 ， 我 们 通常 只 能 通过 网 上 的 一 些 开 
源 项 目 及 其 本 里 的 源 代 人 码 来 了 解 它 。 这 里 需要 提醒 读 者 的 是 ，g20 在 做 
Bundle — Adjustment 的 优化 时 必须 将 其 所 有 点 云 全 部 Schur 挥 ， 合 则 会 出 
fa! 其 原因 在 于 我 们 使 用 了 
g20::LinearSolver<BalBlockSolver::PoseMatrixType> 这 个 类 型 来 指定 
linearsolver， 其 中 模板 参数 当中 的 Pose-MatrixType 在 程序 中 为 相机 姿态 
参数 的 维度 ， 于 是 Bundle Adjustment 当 中 Schur 消 元 后 解 的 线性 方程 组 必 
须 是 只 含有 相机 姿态 变量 由 。 


Ceres 库 则 没有 g2o 这 样 的 限制 。Ceres 给 了 开发 者 很 大 的 空间 去 操作 
目 己 的 优化 策略 ， 它 在 Schur 消 元 操作 时 完全 不 需要 把 所 有 扣 云 都 消 元 
控 ， 用 户 可 以 自己 编写 函数 选择 消 元 哪些 点 云 。 接 下 来 我 们 也 会 给 出 
Ceres 的 BA 示例 。 


10.4 EEk: Ceres 


现在 我 们 再 来 看 看 换 成 Ceres 实 现 同样 的 效果 诅 如 何 做 。 如 前 面 介 
绍 的 那样 ，Ceres 是 针对 一 般 优 化 问题 产生 的 库 ， 因 此 它 并 没有 像 g2o 那 
样 提 供 一 些 Vertex 或 者 Edge 这 类 友好 的 接口 。 不 过 这 也 给 Ceres 囊 来 了 极 
大 的 灵活 性 。 我 们 接 下 来 可 以 看 到 ，Ceres 可 以 在 原始 数组 上 进行 直接 
的 优化 操作 ， 而 g2o 则 通过 复制 的 手段 将 数据 通过 Vertex 或 者 Edge 结 构 
复制 到 optimizer 里 面 。 相 比 之 下 ，Ceres 对 “优化 ”显得 更 贴近 一 些 。 


10.4.1 Ceres k BA 


g20 用 Edge 来 保存 每 一 个 代价 函数 ， 而 Ceres 却 是 用 Problem 类 型 来 
构建 最 终 的 目标 函数 。 与 第 6 讲 介绍 的 一 致 ， 我 们 使 用 AddResidualBlock 
来 添 加 代价 函数 ， 最 后 组 成 整个 目标 函数 。 为 了 人 简化 整个 过 程 ， 我 们 前 
先 定 义 代价 函数 类 型 ， 并 有 日 定义 Create 成 员 来 使 用 Ceres 当 中 的 AutoDi ff 


AP PE: 


kò slambook/ch10/ ceres_custombundle/SnavelyReprojectionError.h 


1 |class SnavelyReprojectionError 


2 |i 
3 | public: 
4 SnavelyReprojectionError(double observation x, double observation, y):observed x 


(observation, x) ,observed_y(observation_y) {} 


6 template<typename T> 

7 bool operator()(const T* const camera, const T* const point, T* residuals)const 
8 1 

9 // camera[0,1,2] are the angle-azis rotation 

10 T predictions[2]; 

11 CamProjectionWithDistortion(camera, point, predictions); 

12 residuals[0] = predictions[0] - T(observed_x) ; 


13 residuals[1] = predictions[1] - T(observed y); 


15 return true; 


18 static ceres::CostFunction* Create(const double observed x, const double 
observed y)í 
19 return (new ceres::AutoDiffCostFunction«SnavelyReprojectionError,2,9,3»( 


20 new SnavelyReprojectionError(observed x,observed y))); 


23 | private: 
24 double observed x; 
25 double observed y; 


26 |}; 





XE Xx. ded lun] DA HjSnavelyReprojectionError::CreaterA Zi 
来 轻松 地 构建 这 个 目标 函数 : 


四 slambook/ch10/ceres_custombundle/ceresBundle.cpp (Fr Ex) 





1 | void BuildProblem(BALProblem* bal, problem, Problem* problem, const BundleParams& 


params) 


2 |i 


3 const int point block size = bal problem-»point block size(); 


4 const int camera block size = bal problem-»camera block size(); 
; double* points = bal_problem->mutable_points() ; 

6 double* cameras = bal, problem-»mutable cameras(); 

7 

8 // Observations is 2 * num observations long array observations 
9 // [u.1, u2, ... un], where each u i is two dimensional, the c 
10 // and y position of the observation. 


11 const double* observations = bal_problem->observations(); 


13 for(int i = 0; i < bal_problem->num_observations(); ++i){ 
14 CostFunction* cost_function; 

15 

16 // Each Residual block takes a point and a camera as input 


17 // and outputs a 2 dimensional Residual 


19 cost function = SnavelyReprojectionError: :Create(observations[2*i + 0], 


observations[2*i + 1]); 


21 // If enabled use Huber's loss function. 

22 LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL; 
23 

24 // Each observatoin corresponds to a pair of a camera and a point 

25 // which are identified by camera indez()[i] and point_index() [i] 

26 // respectively. 

27 double* camera = cameras + camera block size * bal_problem->camera_index() [i]; 
28 double* point = points + point block size * bal_problem->point_index() [i]; 
29 

30 problem-»AddResidualBlock(cost function, loss function, camera, point); 

& |} 


值得 一 提 的 是 ， 为 了 使 用 Schur 消 元 ，Ceres 米 取 的 案 略 和 g20o 有 很 大 
的 不 同 。Ceres 采 用 额外 的 类 型 ParameterBlockOrdering 来 管理 Schur 消 元 
顺序 ， 并 且 使 用 AddElement-ToGroup 来 对 变量 进行 编号 从 而 定义 消 元 顺 
序 。 例 如 下 面 设置 点 云 变 量 为 0， 相 机 变量 为 1 融 可 以 让 点 云 变 量 先 进行 
消 元 “优先 消 元 编写 最 小 的 和 变量) : 


四 slambook/ch10/ceres custombundle/ceresBundle.cpp 


1 | void SetOrdering(BALProblem* bal problem, ceres::Solver::Üptions* options, const 


BundleParams& params) 


2 |i 

3 const int num points = bal problem-»num, points(); 

4 const int point block size = bal problem-»point block size(); 
5 double* points = bal_problem->mutable_points() ; 





7 const int num cameras = bal, problem-»num, cameras(); 

8 const int camera block size = bal, problem-»camera block, size(); 

9 double* cameras = bal, problem-»mutable, cameras(); 

10 

T if (params.ordering -- "automatic") 

12 return; 

13 

14 ceres::ParameterBlockÜrdering* ordering = new ceres::ParameterBlockÜrdering; 
15 

16 // The points come before the cameras 

17 for(int i = 0; i < num points; ++i) 

18 ordering->AddElementToGroup(points + point_block_size * i, 0); 

19 

20 for(int i = 0; i < num_cameras; ++i) 

21 ordering->AddElementToGroup(cameras + camera block size * i, 1); 
22 

23 options->linear_solver_ordering.reset (ordering) ; 

a |} 


10.4.2 求解 


Ceres 除 了 能 够 在 原始 数组 上 操作 以 外 ， 男 一 大 优势 在 于 它 的 优化 
设置 非常 便捷 。g20 的 设置 全 徘 变 量 类 型 来 选择 不 同 的 下 降 寅 略 ， 以 及 
选择 稠密 或 者 稀 牙 的 线性 方程 组 解法 ， 而 Ceres 全 鞭 给 Solver::Options 的 
类 型 成 员 变 量 进 行 赋值 来 调整 ， 这 种 方式 比 g20 快 捷 便利 很 多 。Options 
类 型 几乎 集成 了 Ceres 的 所 有 求解 方法 设置 和 管理 ， 包 括 我 们 上 面 提 到 
的 Schur 消 元 顺序 (注意 上 和 面 疯 数 当 中 的 最 后 一 行 )。 


kh) slambook/ch10/ceres_custombundle/ceresBundle.cpp (FE) 





1 | void SetSolverÜptionsFromFlags (BALProblem* bal problem, 


2 |const BundleParams& params, Solver::Üptions* options)í 


4 options-?max num iterations = params.num iterations; 

5 options-»minimizer progress to stdout = true; 

6 options-»num threads = params.num threads; // 用 于 计算 的 线程 数目 ， 可 以 加 速 雅 可 
比 和 矩阵 的 计算 


8 // 下 降 策 略 的 选取 


9 CHECK (StringToTrustRegionStrategyType(params.trust region, strategy, 

10 &options-^5trust region strategy type)); 

11 

2 // linear solver 的 选取 

13 CHECK (ceres::StringToLinearSolverType(params.linear solver, &options-? 


linear, solver type)); 


14 CHECK(ceres::StringToSparseLinearAlgebraLibraryType(params. 
sparse linear algebra library, &options-»5sparse linear algebra library type)); 
15 CHECK (ceres::StringToDenselinearAlgebraLibraryType(params. 


dense linear algebra library, &options-»5dense linear algebra library type)); 


17 // 变量 排序 的 设置 
18 SetOrdering(bal_problem, options,params) ; 
i9 |} 





CeresHJ 2 f tH f R fe] ER S PAU Hr pe BU AY ARMEA TSE Hn 
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四 slambook/ch10/ceres custombundle/ceresBundle.cpp (FE) 


options.gradient tolerance = 1e-16; 
options.function tolerance = 1e-16; 


Solver::Summary summary; 





Solve(options, &problem, &summary) ; 


和 8g2o 示 例 一 样 ， 该 程序 在 运行 后 也 可 以 在 执行 文件 目录 下 找到 fi 
nal.ply 文 件 和 initial.ply， 由 于 该 程序 和 8g2o 程 序 采 用 一 致 的 数据 ， 因 此 
initial.ply 有 具有 相同 的 图 案 。 使 用 MeshLab 软 件 打开 fi nal.ply 可 以 看 到 如 
图 10-15 所 示 的 结果 ， 通 过 对 比 也 可 以 友 现 优化 结果 和 8g2o 完 全 一 样 。 


ESI 





es 的 优化 结 


Ceres 库 公开 的 API 说 明 详 细 ， 同 时 源 代码 可 读 性 也 局 ， 推 荐 读者 多 
多 阅读 Ceres 源 代码 ， 并 且 目 己 笑 试 在 Schur 消 元 操作 中 只 消去 部 分 护 云 
变量 ， 或 者 夹杂 大 消去 一 些 相 机 变量 。 这 只 需要 操作 
ceres::ParameterBlockOrdering::AddElementToGroup 函 数 ， 在 对 应 变量 地 
址 上， 用 序号 指定 顺序 即 可 。 相 比 g2o 这 样 公开 文档 太 少 的 库 ， 我 们 更 
加 推荐 读者 更 多 地 使 用 Ceres 这 样 的 库 来 做 SLAM。 


图 10-15 Cer 


10.5 “p24 


本 讲 比 较 深 入 地 探讨 了 状态 估计 问题 与 图 优化 的 求解 。 我 们 看 到 在 
经 典 模 型 中 SLAM 可 以 看 成 状态 估计 问题 。 如 果 我 们 假设 马尔 可 夫 性 ， 
只 考虑 当 击 状态， 则 得 到 以 EKF 为 代表 的 滤波 右 檬 型 。 如 石 不 然 ， 我 们 
也 可 以 选择 考虑 所 有 的 运动 和 观测 ， 它 们 构成 一 个 最 小 二 乘 问题 。 在 只 
有 观测 方程 的 情况 下 ， 这 个 问题 称 为 BA， 并 可 利用 非 线 性 优化 方法 求 
解 。 我 们 仔细 讨论 了 求解 过 程 中 的 稀 玩 性 问题 ， 指 出 了 该 问题 与 图 优化 
之 则 的 联系 。 最 后 ， 我 们 演示 了 如 何 使 用 g20o 和 Ceres 库 求解 问 一 个 优化 
问题 ， 让 读者 对 BA 有 一 个 直观 的 认识 。 

习题 

1. 证 明 式 (10.25) 成 并 。 提 示 : 你 可 能 会 用 到 SMW (Sherman- 
Morrison-Woodbury) 公式 ， 参 考 文 献 [76,6]。 

2. 对 比 g20 和 Ceres 的 优化 后 目标 函数 的 数值 。 指 出 为 什么 两 者 在 
Meshlab 中 效 朱 一样 但 数值 却 不 同 。 

3. 对 Ceres 当 中 的 部 分 点 云 进 行 Schur 消 元 ， 看 看 结果 会 有 什么 区 
Fl] 。 

4. 证 明 S EBENE ERE RE. 


5. 阅 读 文 献 [28]， 看 看 g2o 对 核 函 数 是 如 何 处 理 的 。 与 Ceres 中 的 Loss 
function 有 何 联系 ? 


6.* 在 两 个 示例 中 ， 我 们 优化 了 相机 位 姿 、 以 j; ka 为 参数 的 相机 天 


参 及 路 标点 。 请 考虑 使 用 第 5 讲 介 绍 的 完整 的 相机 模型 进行 优化 ， 即 ， 
22/575 FEF, f Pi PK k REE. TEDUIUERN)Ceresfllg20fÉ Hf VATE LS 
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[1] RS SHEA RR. SEN REx , 无关 的 第 数 。 


[2] AWA LSE IN TP ,定义 了 开 ， 又 把 7P ,写成 了 K 的 表达 式 。 然 
而 ， 实 际 当 中 K 可 以 不 依 菲 P, 算得， 参见 本 讲 的 习题 。 


[3] MF VE as HY BEER SEI A BLK AS E] o 


[4] TLE A IER IAT 2E. 捆 集 调整 等 ， 但 笔者 觉得 没有 Bundle Adjustment 这 个 英文 来 得 直观 ， 所 
以 这 里 保留 英文 名 称 。 


[5] 所 谓 邻 接 窍 阵 是 这 样 一 种 窍 阵 ， 它 的 第 订 SOR TIA SI A 是 否 存在 一 条 边 。 如 果 存 在 
W, BOXCAR AL, AAO. 


[6] XF RR SE E LE AR BUR 


lL Jain 


主要 目标 

1. 理 解 Pose Graph 优化 。 

2. 理 解 因子 图 优化 。 

3. 理 解 增 量 式 图 优化 的 工作 原理 。 

4. 通 过 实验 掌握 g20 的 Pose Graph 优 化 与 gtsam 的 因子 图 优化 。 


上 一 讲 我 们 乍 扣 介绍 了 以 BA 为 主 的 图 优化 。BA 能 精确 地 优化 每 个 
相机 位 姿 与 特征 点 位 置 。 不 过 在 更 大 的 场景 中 ， 大 量 特征 点 的 存在 会 严 
重 降低 计算 效率 ， 导 致 计算 量 越 来 越 大 以 全 于 无 法 实时 化 。 本 讲 介绍 两 
种 在 更 大 场景 下 使 用 的 后 中 优化 方法 : 位 姿 图 和 因子 图 。 


11.1 (2244 (PoseGraph ) 


11.1.1 Pose Graph 的 意义 


市 有 相机 位 姿 和 衬 间 点 的 图 优化 称 为 BA， 能 够 有 效 地 求解 大 规模 
的 定位 与 建 图 问题 。 但 是 ， 随 看 时 间 的 流 运 ， 机 大 人 的 运动 轨迹 将 越 来 
越 长 ， 地 图 规模 也 将 不 断 增长 。 像 BA 这 样 的 方法 ， 计 算 效 率 会 〈 令 人 
担忧 地 ) 不 断 下 降 。 根 据 前 面 的 讨论 ， 我 们 发 现 特征 点 在 优化 问题 中 占 
据 了 绝 大 部 分 。 而 实际 上 ， 经 过 奢 干 次 观测 之 后 ， 那 些 收 合 的 特征 点 ， 
空间 位 置 佑 计 会 收 征 至 一 个 值 保 持 不 动 ， 而 发 散 的 外 点 则 通 党 看 不 到 
了 。 对 收 伍 点 再 进行 优化 ， 似 乎 是 有 些 费 力 不 讨 好 的 。 因 此 ， 我 们 更 倾 
器 于 在 优化 几 次 之 后 束 把 特征 点 固定 住 ， 只 把 它们 看 作 位 姿 估计 的 约 
束 ， 而 不 再 实际 地 优化 它们 的 位 置 估计 。 

治 看 这 个 思路 往 下 走 ， 我 们 会 想到 : 是 否 能 够 完全 不 害 跤 标 而 只 官 
轨迹 呢 ? 我 们 完全 可 以 构建 一 个 只 有 轨迹 的 网 优化 ， 而 位 姿 贡 点 之 间 的 
边 ， 可 以 由 两 个 关键 帆 之 间 通 过 特征 匹配 之 后 得 到 的 运动 估计 来 给 定 初 
始 值 。 不 同 的 是 ， 一 旦 初始 估计 完 成 ， 我 们 吏 不 再 优化 那些 路 标点 的 位 
置 ， 而 只 关心 所 有 的 相机 位 姿 之 间 的 联系 了 。 通 过 这 种 方式 ， 我 们 省 去 
了 大 量 的 特征 点 优化 的 计算 ， 只 保留 了 关键 闫 的 轨迹 ， 从 而 构建 了 上 所谓 
的 位 姿 图 (Pose Graph) ， 如 网 11-1 所 示 。 
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Bundle Adjustment Pose Graph 


图 11-1 Pose Graph 示意 图 。 当 我 们 不 再 优化 Bundle Adjustment 中 的 路 标点 ， 仅 把 它们 看 
成 对 姿态 节点 的 约束 时 ， 束 得 到 了 一 个 计算 规模 减 小 很 多 的 Pose Graph. 


我 们 知道 ， 在 BA 中 特征 点 数量 远大 于 位 姿 节 点 。 一 个 关键 帧 往往 
天 联 了 数 百 个 关键 点 ， 而 实时 BA 的 最 大 计算 规模 ， 即 使 利用 稀疏 性 ， 
在 当前 的 主 沁 CPU 上 一 般 也 束 是 几 万 个 点 左右 。 这 就 限制 了 SLAM 应 用 
场景 。 所 以 ， 当 机 器 人 在 更 大 范围 的 时 间 和 和 至 间 中 运动 时 ， 必 须 考 虑 一 
些 解决 方式 : BABI) BOERS, ARE PAE ; BAR 
Pose Graph (CAA, BATE Sh, AER Posez lal Ay, 
使 用 Pose GraphU9790! 。 


11.1.2 Pose Graph 的 优化 


ASA, Pose Graph 图 优化 中 的 和 点 和 边 都 是 什么 意思 呢 ? A 
点 表示 相机 位 姿 ， 以 5 5 RAIA. IW, WE PY Pees a TA FA 
运动 的 估计 ， 广 估计 可 能 来 和 目 于 特征 点 法 或 直接 法 ， 但 不 党 如 何 ， 我 们 
估计 了 ， 比 如 说 和 和 5 之 间 的 一 个 运动 A6，。 访 运动 可 以 有 厂 干 种 表达 
方式 ， 我 们 取 比 较 目 然 的 一 种 : 


V 


Af 6 o£; = ln (exp (én) exp (62))  , (11.1) 
PFE HE AY A: 
AT,,; = T, T; (11.2) 


按照 图 优化 的 思路 来 看 ， 实 际 当 中 该 等 式 不 会 精确 地 成 立 ， 因 此 我 
们 设立 最 小 二 乘 误 着 ， 然 后 和 以 往 一 样 ， 讨 论 误 着 天 于 优化 变量 的 导 
数 。 这 里 ， 我 们 把 上 式 的 AT BASS VAM, Mize, : 


ej = (Am) 
= In (exp((—£;5)^) exp((—&;)^) exp(£^)) ' . 
注意 优化 变量 有 两 个 ，5 和 5 ， 因 此 我 们 求 e; 关于 这 两 个 变量 的 导 
数 。 按 照 李 代数 的 求 导 方式 ， 给 5 ME 各 一 个 左 扰动 : 65 和 65 。 于 是 
误差 变 为 


(11.3) 


éij = In (T; T; exp((—8&;)^) exp(0£2)T;)' . (11.4) 
该 式 中 ， 两 个 扰动 项 钻 夹 在 了 中 间 。 为 了 利用 BCH 近 似 ， 我 们 和 希望 


把 扰动 项 移 至 式 于 元 侧 或 右 侧 。 回 忆 第 4 讲习 题 中 的 伴随 性 质 ， 即 陈 
(4.49)。 如 末 你 没有 做 过 这 个 习题 ， 那 融 特 时 想当然 地 认为 其 正确 。 


exp ((Ad(D)£)^) = T exp(£^)T" |. (11.5) 
WAE, A: 

exp(£^)T = T exp ((Ad(GT-DS) ) | (11.6) 
该 式 表明 ， 通 过 引入 一 个 伴随 项 ， 我 们 能 够 “交换 ”扰动 项 左右 侧 的 


T 。 利 用 它 ， 可 以 将 扰动 挪 到 最 右 〈 当 然 最 左 亦 可 〉 ， 寻 出 右 乘 形 却 的 
AE PY EGER CGS BI Ae I AZ eM Ae HE): 


éi; In (d exp((—ó6;)^) exp(0£^)T;)' 
— In CURE EA exp ((-Aa(r; 86; ) exp((Ad(T- *)6E;) 
~ In (Ty TT, [I — (Ad(T; ')8&;)^  (Ad(T; )66)^])" 


58 ze t ee =~ OE; + + 5g 0&; 


因此 ， 按 照 李 代数 上 的 求 导 法 则 ， 我 们 求 出 了 误差 关于 两 个 位 姿 的 
MENJ EERE, KFT, HN: 


(11.7) 








Óei; Lo mT-—líL. 一 1 

Boe 一 JT, (eij)Ad(T; ). (11.8) 
DUO T, f 

A T lenad T" (11.9) 

00€; did To ' 


如 条 读者 觉得 这 部 分 求 寻 理解 起 来 有 困难 ， 可 以 回 到 第 4 讲 温 习 一 


下 李 代 数 部 分 的 内 容 。 不 过 ， 前 面 也 说 过 ， 由 于 se(3) 上 的 左右 雅 可 比 了 
形式 过 于 复杂 ， 我 们 通常 取 它 们 的 近似 。 如 果 误 差 接 近 于 零 ， 我 们 就 可 
以 设 它 们 近似 为 I 或 


Pe Pe | (11.10) 


理论 上 讲 ， 即 使 在 优化 之 后 ， 由 于 每 条 边 给 定 的 观测 数据 并 不 一 
DO RAE A WEI, PUA Te PEIN, 设置 为 1 会 有 
一 定 的 损失 。 和 后 我 们 将 退 过 实践 来 看 看 理论 上 的 区 列 是 任 明 显 。 

了 解 雅 可 比 求 寻 后 ， 剩 下 的 部 分 承 和 普通 的 图 优化 一 样 了 。 人 简 而 言 
之 ， 所 有 有 的 位 姿 项 点 和 位 姿 一 一 位 姿 边 构成 了 一 个 图 优化 ， 本 质 上 是 一 
个 最 小 二 乘 问题 ， 优 化 变量 为 各 个 项 点 的 位 姿 ， 边 来 自 于 位 姿 观 测 约 
Ro We 为 所 有 边 的 集合 ， 那 么 电 体 目标 孙 数 为 





me | T «7—1 
E^ E E 订 之 ij e. (11.11) 
i,j€€ 


Til VERA n] VA FB i AR. SCR Sa OR TT VES SOR fie 
8l, ER Y PINRO, MAAEMA. ARG SC BU AS 
验 ， 这 目 然 可 以 用 Ceres 或 g2o 进 行 求解 。 我 们 不 再 讨论 优化 的 详细 过 
程 ， 上 一 讲 已 经 讲 得 够 多 了 。 


11.2 实践: 位 姿 图 优化 


11.2.1 g20 原 生 位 姿 图 


下 和 面 演 示 使 用 g2o 进 行 位 次 图 优化 。 前 先 ， 请 庆 者 用 g20_viewer 打 
开 我 们 了 预先 生成 的 仿真 位 姿 图 ， 位 于 slambook/ch11/sphere.g20 中 ， 如 图 
11-2ÀT7n. 





Odometry 





Loop Closure 


Add Noise 








图 11-2 g2 RAF Enea. Abe sce, TEA ELAR SITE AR 


于 的 仿真 数据 。 


该 位 姿 图 是 由 g20 自 带 的 create sphere 程 序 仿 真 生 成 的 。 它 的 真实 轨 
迹 为 一 个 球 ， 由 从 下 往 上 的 多 个 层 组 成 。 每 层 为 一 个 正 圆 形 ， 很 多 个 大 
小 不 一 的 圆 形 层 组 成 了 一 个 完整 的 球体 ， 共 包含 2500 个 位 姿 节 点 (图 
11-27E E) ， 可 以 看 成 一 个 转 较 上 升 的 过 程 。 然 后 ， 仿 真 程序 生成 了 t- 1 
Ft 时刻 的 边 ， 称 为 odometry 边 (里 程 计 ) 。 此 外 ， 叉 生成 层 与 层 之 间 
的 边 ， 称 为 loop closure 回环， 将 在 下 一 讲 详细 介绍 ) 。 随 后 ， 在 每 条 
边 上 添加 观测 噪声 ， 并 根据 里 程 计 边 的 噪声 ， 重 新 设置 节点 的 初始 值 。 
这 样 ， 束 得 到 了 市 索 积 误 才 的 位 姿 图 数据 (图 11-2 右 下 〉 。 它 局 部 看 起 
来 像 球 体 的 一 部 分 ， 但 整体 形状 与 球体 相关 其 远 。 现 在 我 们 从 这 些 训 噪 
声 的 边 和 节点 初始 值 出 及 ， 符 试 优化 整个 位 姿 图 ， 得 到 近似 真 值 的 数 
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当然 ， 实 际 当 中 的 机 需 人 肯定 不 会 出 现 这 样 正 球形 的 运动 轨迹 ， 以 
及 如 此 完整 的 里 程 计 与 回环 观测 数据 。 仿 真 成 正 球 的 好 处 是 我 们 能 够 二 
观 地 看 到 优化 线条 征 人 否 正确 《只 要 看 它 各 个 角度 圆 不 圆 束 行 了 ) 。 读 者 
可 以 单 击 g2o_Vviewer 中 的 optimize 函 数 ， 看 到 每 步 的 优化 结束 和 收敛 的 
过 程 。 态 一 方面 ，sphere.g2o 也 十 一 个 文本 文件 ， 可 以 用 文本 编辑 套 打 
开 ， 答 看 它 里 面 的 内 容 。 文 件 前 半 部 分 由 布 氮 组 成 ， 后 半 部 分 则 十 过 : 





1 | VERTEX_SE3:QUAT 0 -0.125664 -1.53894e-17 99.9999 0.706662 4.32706e-17 0.707551 
-4.3325e-17 


3 | EDGE, SES:QUAT 1524 1574 -0.210399 -0.0101193 -6.28806 -0.00122939 0.0375067 


-2.85291e-05 0.999296 1000000000 10000 O O O O 100000 O 0 40000 0 0 40000 0 
40000 


可 以 看 到 ， 节 点 类 型 是 VERTEX_SE3， 表 达 一 个 相机 位 姿 。g2o 默 
认 使 用 四 元 数 和 平移 同 量 表达 位 姿 ， 所 以 后 面 的 字段 意义 为 : ID, t, st, 
, ,qx qy qz ,q，。 前 3 个 为 平移 同 量 元 素 ， 后 4 个 为 表示 旋转 的 单位 四 元 
数 。 同 样 ， 边 的 信息 为 两 个 节点 的 ID,， £,6,6,q,,0,.0,.q, ， 信 息 矩 阵 的 
右上 角 “《 由 于 信息 窍 阵 为 对 称 阵 ， 只 需 保 存 一 半 即 可 ) 。 可 以 看 到 这 里 
把 信息 矩阵 设 成 了 对 角 阵 。 


为 了 优化 该 位 姿 图 ， 我 们 可 以 使 用 g2o 秋 认 的 顶点 和 边 ， 它 们 十 由 
四 元 数 表 示 的 。 由 于 仿真 数据 也 是 g20 生 成 的 ， 所 以 用 g2o0 本 和 丑 优化 就 无 
须 我 们 多 做 什么 工作 了 ， 只 需 配 置 一 下 优化 参数 即 可 。 程 序 
slambook/ch11/pose graph g20o SE3.cppisBizm [f QO (A) RH x 4Hte-——— 
尔 特 方法 对 该 位 姿 图 进行 优化 ， 并 把 结果 存储 至 result.g2o 文 件 中 。 


四 slambook/ch1 1/pose_graph_g20_SE3.cpp 





1 | #include 
2 | #include 


3 | #include 


5 | #include 
6 | #include 
7 | #include 
8 | #include 
9 | #include 


10 | #include 


<iostream> 
<fstream> 


<string> 


<g20/types/slam3d/types_slam3d.h> 
<g20/core/block_solver.h> 
<g20/core/optimization_algorithm_levenberg.h> 
<g20/core/optimization_algorithm_gauss_newton.h> 
<g20/solvers/dense/linear_solver_dense.h> 


<g20/solvers/cholmod/linear_solver_cholmod.h> 


11 |using namespace std; 


13 | Joooeleelololeleololeleloeloieeleleleleeleleloleleleleleeleleloleeleeleelelelereree 
u [x 本 程序 演示 如 何 用 g2o solver 进行 位 次 图 优化 
15 | * sphere.g20 是 人 工 生 成 的 一 个 Pose graph ， 我 们 来 优化 它 。 
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* 尽管 可 以 直接 通过 load 函数 读 取 整 个 图 ， 但 我 们 还 是 自己 来 实现 读 取 代码 ， 以 期 获得 更 深 


刻 的 理解 。 
* 这 里 使 用 g2o/types/slam3d/ 中 的 SE3 表示 位 姿 ， 它 实质 上 是 四 元 数 而 非 李 代数 


* ak oc eoe oo oe ok oe oe ok oko oe je oe ok ok ope oe ok oko oe ojo ok ok oe ok oe oe oe oko ooo eoe tek T 


int main( int argc, char** argv ) 


1 


if ( argc != 2) 


1 
cout««"Usage: pose graph g20o SES3 sphere.g2o"««endl; 
return 1; 

} 

ifstream fin( argv[i] ); 

if ( !fin ) 

{ 
cout<<"file "<<argv[1]<<" does not exist."««endl; 
return 1; 

} 


typedef g20::BlockSolver<g2o0: :BlockSolverTraits<6,6>> Block; // 626 
BlockSolver 

Block: :LinearSolverType* linearSolver = new g20o::LinearSolverCholmod«Block:: 
PoseMatrixType»(); 

Block* solver ptr = new Block( linearSolver ); // FETE RRS 
g20::0ptimizationAlgorithmLevenberg* solver-new g2o:: 
OptimizationAlgorithmLevenberg(solver ptr); 

g20::SparseOptimizer optimizer; // 图 模型 
optimizer.setAlgorithm( solver ); // 设置 求解 器 


int vertexCnt = 0, edgeCnt = 0; // 顶点 和 边 的 数量 
while ( !fin.eof() ) 
1 
string name; 
fin>>name; 
if ( name == "VERTEX_SE3:QUAT" ) 
{ 
// SES 顶点 
g20::VertexSE3* v = new g20o::VertexSE3(); 
int index = O0; 
fin>>index; 
v->setId( index ); 
v->read (fin) ; 
optimizer.addVertex(v); 
vertexCnt++; 
if ( index==0 ) 


57 v-»setFixed(true); 


58 J 

59 else if ( name=="EDGE_SE3:QUAT" ) 

60 { 

61 // SE3-SE3 ià 

62 g20::EdgeSE3* e = new g20o::EdgeSE3() ; 

63 int idxi, idx2; // 关联 的 两 个 顶点 

64 fin>>idx1>>idx2: 

65 e->setId( edgeCntt+ ); 

66 e->setVertex( 0, optimizer.vertices()[idxi] ); 
67 e->setVertex( 1, optimizer.vertices()[idx2] ); 
68 e->read(fin) ; 

69 optimizer .addEdge(e) ; 

70 } 

71 if ( !fin.good() ) break; 

72 } 

73 

74 cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges."««endl; 
75 

76 cout<<"prepare optimizing ..."««endl; 

77 optimizer.setVerbose(true); 

78 optimizer.initializeOptimization() ; 

79 cout<<"calling optimizing ..."««endl; 

80 optimizer.optimize(30); 

81 

82 cout<<"saving optimization results ..."««endl; 

83 optimizer.save("result.g20"); 

84 

85 return 0; 

86 |} 


我 们 选择 了 6x CHR aS, BEAVIS RSS RE PEUT X 
TEAR BLE FE SOUR o Y SCE TOS DES REITA: 





$ build/pose_graph_g20_SE3 sphere.g2o 


read total 2500 vertices, 9799 edges. 
prepare optimizing ... 


calling optimizing .. 


iteration= 0 chi2= 1023011093.967638 time= 0.851879 cumTime= 0.851879 edges= 9799 


schur= 0 lambda- 805.622433 levenbergIter- 1 
iteration- 1 chi2= 385118688.233188 time= 0.863567 cumTime- 1.71545 
schur- 0 lambda- 537.081622 levenbergIter- 1 
iteration- 2 chi2= 166223726.693659 time= 0.861235 cumTime- 2.57668 
schur= 0 lambda- 358.054415 levenbergIter= 1 
iteration- 3 chi2= 86610874.269316  time- 0.844105 cumTime- 3.42079 


schur= 0 lambda- 238.702943 levenberglter= 1 
iteration- 4 chi2= 40582782.710190  time- 0.862221 cumTime- 4.28301 
schur- 0 lambda- 159.135295 levenbergIter= 1 


edges= 9799 


edges- 9799 


edges= 9799 


edges= 9799 


iteration- 28 chi2- 45095.174398 time- 0.869451 cumTime- 30.0809 edges- 9799 schur- 


0 lambda- 0.003127 levenbergIter- 1 


iteration- 29 chi2- 44811.248504 time- 1.76326 cumTime= 31.8442 edges- 9799 schur- 


0 lambda- 0.003785 levenbergIter= 2 


saving optimization results ... 





然后 ， 用 g2o_viewer 打 开 result.g2o 丛 看 结果 ， 如 网 11-3 所 示 。 
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图 11-3 ”使 用 g20 目 市 的 项 点 与 边 求解 的 结果 。 


结果 从 一 个 不 规则 的 形状 优化 成 了 一 个 看 起 来 完整 的 球 。 这 个 过 程 
实质 上 和 我 们 单 击 g2o_viewer 上 的 Optimize 按 钮 没有 区 别 。 下 面 ， 我 们 
根据 前 面 的 李 代 数 推导 来 实现 一 下 李 代 数 上 的 优化 。 


11.2.2” 李 代数 上 的 位 姿 图 优化 
还 记得 我 们 用 Sophus 来 表达 人 至 代数 的 事情 吗 ? 我 们 来 试 试 把 Sophus 


用 到 g20o 中 定义 目 己 的 项 点 和 边 吧 。 
^] slambook/ch1 l/pose graph g20 lie algebra.cpp (Fr Ec) 


#include 
#include 
#include 


#include 


#include 
#include 
#include 
#include 
#include 
#include 
#include 


#include 


#include 


#include 


<iostream> 
<fstream> 
<string> 


<Eigen/Core> 


<g20/core/base_vertex.h> 
<g20/core/base_binary_edge.h> 
<g20/core/block_solver.h> 
<g20/core/optimization_algorithm_levenberg.h> 
<g20/core/optimization_algorithm_gauss_newton.h> 
<g20/core/optimization_algorithm_dogleg.h> 
<g20/solvers/dense/linear_solver_dense.h> 


<g20/solvers/cholmod/linear_solver_cholmod.h> 


<sophus/se3.h> 
<sophus/so3.h> 


using namespace std; 


using Sophus::SE3; 
using Sophus::S03; 


GIGI III IO IO IO III IO IOI I TOR IOI AIR IAI 
* 本 程序 演示 如 何 用 g2o solver 进行 位 姿 图 优化 
* sphere.g20 是 人 工 生 成 的 一 个 Pose graph ， 我 们 来 优化 它 。 


* 尽管 可 以 直接 通过 Load 函数 读 取 整 个 图 ， 但 我 们 还 是 自己 来 实现 读 取代 码 ， 以 期 获得 更 深 


刻 的 理解 。 
* 本 节 使 用 车 代数 表达 位 姿 图 ， 节 点 和 边 的 方式 为 自 定 义 。 


* ak ok eje ok ok oe oe oe oj oe ok a oe oe ok oj oe obe ok oe sje oe ok ak ak oe ok ak ak ok ok ak kak ak ak ok ak ak ak ak ake ak // 


typedef Eigen::Matrix«double,6,6» Matrix6d; 


// 给 定 误差 求 J R^(-1) 的 近似 
Matrix6d JRInv( SE3 e ) 


1 
Matrix6d J; 
J.block(0,0,3,3) = S03::hat(e.so3().log()); 
J.block(0,3,3,3) = S03::hat(e.translation()); 
J.block(3,0,3,3) = Eigen: :Matrix3d: :Zero(3,3); 
J.block(3,3,3,3) = S03::hat(e.so3().log()); 
J = J*0.5 + Matrix6d::Identity(); 
return J; 

} 


// 李 代 数 顶 点 
typedef Eigen::Matrix«double, 6, 1» Vector6d ; 
class VertexSE3LieAlgebra: public g20::BaseVertex«6, SE3> 


1 
public: 
EIGEN MAKE, ALIGNED. OPERATOR. NEW 
bool read ( istream& is ) 
1 
double data[T7]; 
for ( int i20; i<7; i++ ) 
is>>data[i]; 
setEstimate ( SE3 ( 
Eigen::Quaterniond ( data[6],data[3], data[4], data[5] ), 
Eigen::Vector3d ( data[0], data[1], data[2] ) 
2323 


bool write ( ostream& os ) const 


1 
os««1d()««" ":; 
Eigen::Quaterniond q =  estimate.unit quaternion(); 
os«« estimate.translation().transpose()««" "; 
os<<q.coeffs() [0]<<" "««q.coeffs()[1]««" "<<q.coeffs() [2]««" "««q.coeffs() 
[3] ««end1; 
return true; 
) 


virtual void setToOriginImpl.() 


1 
_estimate = Sophus::SE3(); 
h 
// 左 来 更 新 
virtual void oplusImpl ( const double* update ) 
1 
Sophus::SE3 up ( 
Sophus::S03 ( update[3], update[4], update[5] ), 
Eigen::Vector3d ( update[0], update[1], update[2] ) 
) ; 
.estimate = up* estimate; 
} 


J; 


// 两 个 李 代数 节点 之 边 
class EdgeSE3LieAlgebra: public g2o0::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, 
VertexSE3LieAlgebra> 
t 
public: 
EIGEN MAKE ALIGNED OPERATOR NEW 


bool read ( istream& is ) 


1 


) 


double data[7]; 
for ( int i=0; i«7; i++ ) 
is»»data[i]; 
Eigen::Quaterniond q ( data[6], data[3], data[4], data[5] ); 
q.normalize(); 
setMeasurement ( 
Sophus::SE3 ( q, Eigen::Vector3d ( data[0], data[i], data[2] ) ) 
); 
for ( int i=0; i<information().rows() && is.good(); i++ ) 
for ( int j=i; j<information().cols() && is.good(); j++ ) 
{ 
is >> information() ( i,j ); 
if ( i!2j ) 
information() ( j,i ) =information() ( i,j ); 
} 


return true; 


bool write ( ostream& os ) const 


1 


VertexSE3LieAlgebra* vi = static cast«VertexSE3LieAlgebra*» ( vertices[0]); 
VertexSE3LieAlgebra* v2 = static cast«VertexSE3LieAlgebra*» ( vertices[1]); 
os««vi-»id()««" "««v2-»id()««" "; 


SE3 m » measurement; 

Eigen::Quaterniond q - m.unit quaternion(); 
os««m.translation().transpose()««" "; 

os««q.coeffs()[0]««" "««q.coeffsO [1]««" "<<q.coeffs() [2] ««" "<<q.coeffs() 
[3]««" " ; 

// information matriz 

for ( int i20; i<information().rows(); i++ ) 


for ( int j=i; j<information().cols(); j** ) 


1 
os << information(O ( i,j J << " '*; 
} 
os««endl; 


return true; 


// 误差 计算 与 书 中 推导 一 臻 


virtual void computeError() 


1 


Sophus::SE3 vi 
estimate(); 
Sophus::SE3 v2 


(static cast«VertexSE3LieAlgebra*» ( vertices[0]))-» 


(static cast«VertexSE3LieAlgebra*» ( vertices[1]))-» 


estimate(); 


130 _error = ( measurement.inverse()*vi.inverse()*v2).log(); 

131 } 

132 

133 // 雅 可 比 计算 

134 virtual void linearizeÜplus() 

135 1 

136 Sophus::SE3 vi = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))-> 
estimate() ; 

137 Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> ( vertices[1]))-? 
estimate(); 

138 Matrix6d J = JRInv(SE3::exp( error)); 

139 // ZAJE A I? 

140 _jacobianOplusXi = - J* v2.inverse().AdjO; 

141 _jacobianOplusXj = J*v2.inverse().Adj(); 

142 } 

43 |}; 


73 f EMX g20 LR AER, EAEE, f read fll write ES 
T FHAR” Kg A SB WSESUG, füfdg2o viewerHE ge V VH JF TH Xe 
它 。 事 实 上 ， 除 了 内 部 使 用 Sophus 的 他 代数 表示 之 外 ， 从 外 部 看 起 来 没 
HAT AE). 

值得 注意 的 是 这 里 雅 可 比 的 计算 过 程 。 我 们 有 厂 干 种 选择 一 是 不 
提供 雅 可 比 计算 函数 ， 让 g2o 目 动 计 算数 值 雅 可 比 。 二 是 提供 完整 或 近 
似 的 雅 可 比 计 算 过 程 。 这 里 我 们 用 JRInv0O 函 数 提 供 近 似 的 7zr:。 读者 可 
以 符 试 把 它 近 似 为 ， 或 者 干脆 注释 反 oplusImpl 函 数 ， 看 看 结果 会 有 什 
ZX Fill 


之 后 调用 g20 进 行 优 化 问题 : 


] 


法 俘 止 了 。 而 上 一 个 实验 中 ， 用 满 了 30 次 适 代 后 误 大 仍 在 下 降 上 。 在 


$ build/pose graph g2o lie Sphere.g2o 


read total 2500 vertices, 9799 edges. 


prepare optimizing ... 
calling optimizing ... 
chi2- 781963143.389706 time- 1.9322 


iteration= 0 


schur= 0 lambda= 6706.585223 levenbergIter- 1 


iteration= i 


chi2= 236521032.458035 time= 1.91309 


schur= 0 lambda= 2235.528408 levenbergIter= 1 


iteration= 2 


chi2= 142934798.398778 time= 1.9792 


schur= 0 lambda= 745.176136  levenbergIter- 1 


iteration= 3 


chi2= 84490229.050137 time= 2.03394 


schur= 0 lambda= 248.392045 levenbergIter= 1 


iteration= 4 
schur= 0 lambda= 82 


iteration= 21 chi2= 


ij | iteration= 22 chi2= 127578. 
schur= 0 lambda= 0.000237 
13 | iteration- 23 chi2- 127578. 
schur= 0 lambda= 0.000079 
i4 | iteration= 24 chi2= 127578. 
schur= 0 lambda= 0.000053 
is | iteration- 25 chi2= 127578. 
schur= 0 lambda= 0.000035 
16 | iteration- 26 chi2= 127578. 
schur= 0 lambda= 0.000023 
17 | iteration= 27 chi2= 127578. 
schur= 0 lambda= 0.000031 
is | iteration- 28 chi2= 127578. 
schur= 0 lambda= 0.000042 
19 | iteration= 29 chi2- 127578. 
schur= 0 lambda= 0.001779 


127607 . 121623 
schur= 0 lambda= 0.000712 


chi2= 42690811.624643 time= 2.05149 


levenbergIter- 1 


time- 1.9686 


levenbergIter- 1 


889888 time= 1.94773 
levenbergIter- 1 
158794 time- 1.98009 


levenbergIter- 1 
157859 2.04546 
levenbergIter- 1 
157859 1.96722 
levenbergIter- 1 
157859 2.11235 
levenbergIter- 1 
157859 3.29151 
levenbergIter- 2 
157859 3.20302 
levenbergIter- 2 
157859 5.56337 


levenbergIter- 4 


time- 


time- 


time- 


time- 


time- 


time- 


2 | Saving optimization results ... 


cumTime= 


cumTime= 


cumTime- 


cumTime- 


cumTime- 


cumTime- 


cumTime= 


cumTime= 


45 


47 


49. 


51 


53. 


56. 


60. 


65. 


cumTime= 1.9322 


cumTime= 3.84529 


cumTime= 5.8245 


cumTime= 7.85844 


cumTime= 9.90993 


cumTime= 43.526 


.4737 


,4538 


4993 


.4665 


5789 


8704 


0734 


6368 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


edges- 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 
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Vi 


用 优化 后 ， 碍 看 result_lie.g2o 观 察 它 的 结 末 ， 如 图 11-4 所 示 。 从 肉眼 上 
看 不 出 任何 区 别 。 


g20 Viewer 





v/ Draw Axis 
# Iterations 
10 
Robust Kernel 
eq ientia 
| Cauchy 


Kernel Width 


Optimizer 


gn_var_cholmod 


Parameters 


Spanning Tree 





Initial Guess 
SetZero 
Optimize loaded result lie.g20 with 2500 vertices and 9799 measurements 
graph is fixed by node 2499 
Quit 


图 11-4 SEE EC ka Se g 


如 果 你 在 这 个 g2o_viewer 界 面 按 下 Optimize 按 钮 ，g2o 将 使 用 它 自 带 
的 SE3 顶 点 进行 优化 ， 你 可 以 在 下 方 文本 框 中 看 到 : 


1 | loaded result lie.g20 with 2500 vertices and 9799 measurements 
2 |graph is fixed by node 2499 

3 |# Using CHOLMOD poseDim -1 landMarkDim -1 blockordering 

4 | Preparing (no marginalization of Landmarks) 


5 |iteration- 0 chi2= 44360.509723 time= 0.567504 cumTime= 0.567504 edges- 9799 schur- 


0 

6 |iteration- 1 chi2- 44360.471110 time- 0.595993 cumTime- 1.1635 edges= 9799 schur= 
0 

7 | iteration- 2 chi2= 44360.471110 time- 0.582909 cumTime- 1.74641 edges= 9799 schur= 
0 





整体 误差 在 SE3 边 的 度量 下 为 44360， 略 小 于 之 前 30 次 迭代 时 的 


44811。 这 说 明 使 用 孚 代数 进行 优化 后 ， 我 们 在 更 少 的 帮 代 次 数 下 得 到 
了 更 好 的 结 打 所 。 


11.2.3 ”小 结 


球 的 例子 是 一 个 比较 有 代表 性 的 采 例 。 它 具有 和 实际 中 相似 的 里 程 
计 边 (Odometry〉 和 回环 边 〈LoopClosure) ， 这 也 正 是 实际 SLAM 中 一 
个 位 姿 图 中 可 能 有 的 东西 。 同 时 ，“ 球 ”也 具有 一 定 的 计算 规模 : 它 总 共 
有 2;500 个 位 姿 贡 点 和 近 10,000 条 边 ， 我 们 发 现 优化 它 这 了 不 少时 间 《〈 相 
TF SEI PE BEE RA A vi Eo TT, KU ALAA Ea 
好 人 简 早 的 图 之 一 。 在 我 们 不 假设 机 右 人 如 何 运动 的 击 提 下 ， 很 难 再 进 一 
Vow en mie T ALA Las A A] fe BE Ie, TERRY 
位 姿 图 ， 是 稀 牙 的 ;也 可 能 是 “左手 右手 一 个 慢 动 作 ”， 形 成 大 量 的 小 型 
回环 需要 优化 〈Loopy motion) ， 从 而 变 成 像 “ 球 ”那样 比较 笛 答 的 位 姿 
图 。 无 论 如 何 ， 在 没有 进一步 的 信息 之 前 ， 我 们 似乎 无 法 再 利用 位 姿 图 
的 求解 结构 了 。 


目 PTAMIL 提出 以 来 ， 人 们 残 意 识 到 ， 后 器 的 优化 没 必 要 实时 地 咱 
应 前 器 的 网 像 数据 。 人 们 倾 癌 于 把 前 闫 和 后 站 分 开 ， 运 行 于 两 个 独立 线 
程 之 中 ， 历 史上 称 为 跟踪 (Tracking) MÆR (Mapping) 虽然 如 
此 叫 ， 建 图 部 分 主要 是 指 后 端的 优化 内 容 。 通 俗 地 说 ， 前 内需 要 实时 啊 
应 视频 的 速度 ， 例 如 每 秒 30 巾 ;而 优化 可 以 慢 修 悠 地 运行 ， 只 要 在 优化 
完成 时 把 结果 返回 给 前 端 即 可 。 上 所 以 我 们 通 第 不 会 对 后 闪 优 化 提出 很 高 
的 速度 要 求 。 








11.3. *| 因 子 图 优化 初步 


11.3.1 Ulm re] 2 


Fm. RIAA -TAEKE Ja mitt: Aris iy Al K 
(FactorGraph〉 优 化 。 由 于 这 部 分 内 容 罕 涉 到 概率 图 理论 ， 超 出 了 本 书 
沁 围 ， 所 以 我 们 只 能 大 概 地 介绍 如 何 从 概率 图 角度 来 看 竺 这 个 问题 。 如 
果 读 者 对 概率 图 模型 感 兴趣 ， 建 议 阅 读 文 献 [82]。 或 者 ， 如 果 对 如 何 使 
用 因子 图 实现 SLAM 后 端 优化 的 细节 感 兴趣 ， 可 以 阅读 文献 
[83,84,85,86 |]. 


从 贝 时 斯 网 络 (Bayes Network) 8348 BE2K&, SLAM HJ UJ A ZAR 
达成 一 个 动态 贝 叶 斯 网 络 (Dynamic Bayes Network, DBND . 与 图 优 
化 类 似 ， 贝 叶 斯 网 络 是 一 种 概率 图 ， 由 随机 变量 的 节点 和 表达 随机 变量 
条 件 独 立 性 的 边 组 成 ， 形 成 一 个 有 癌 无 坏 图 (Directed Acyclic 
Graph) 。 在 SLAM 中 ， 由 于 我 们 有 运动 方程 和 观测 方程 ， 它 们 恰好 表 
示 了 状态 变量 之 间 的 条 件 概 率 ， 如 图 11-5 所 示 。 





图 11-5 ”以 贝 叶 斯 网 络 形式 表达 的 SLAM 过 程 示 意图 。 竖 线 为 运动 方程 ， 其 他 线 为 观测 方 
程 。 虚 线 框 表 示 一 次 观测 ， 实 线 框 表 示 一 次 运动 。 


图 11-5 中 的 圆圈 表示 了 贝 叶 斯 网 络 的 节点 ， 也 融 是 与 图 相关 的 随机 


变量 ， 包 括 : 

LFA DUCATI RAE ER: X1X，…。 

2.55] AN ECT EA: Ue 

3. EE B RA: lo 

AJ INST RR: ze 

这 组 成 了 所 有 与 SLAM 过 程 相关 的 信息 。 另 一 方面 ， 各 线条 表示 了 
它们 之 则 的 关系 ， 和 荫 头 表示 依赖 天 系 。 例 如 ， 从 x | tx, BERK, W 
明 x， 依赖 于 x;， ， 此 边 表示 的 概率 是 P (x ,|x 1 ) 一 一 事实 上 ， 运 动 方程 指 上 
I Ax, Plx, 的 运动 和 关系。 我 们 观察 实 线 框 。 这 个 框 中 ， 变 量 x , 依赖 于 x 
U, o 回忆 运 动 方程 : 


T3 = f (2:2, U3) + t3. (11.12) 
它 实际 上 给 出 了 这 几 个 变量 的 条 件 概率 的 度量 : 
P(£3|£2, U3). (11.13) 


同样 ， 虚 线 框 中 的 一 次 观测 ， 亦 说 明 观 测 方 程 给 出 了 变量 间 的 条 件 
概率 关系 : 


P(21|%1,1,). (11.14) 


通过 这 种 方式 ， 我 们 构建 了 一 个 贝 叶 斯 网 络 ， 它 表达 了 所 有 变量 ， 
以 及 各 个 方程 给 出 的 变量 之 间 的 条 件 概率 关系 。 请 注意 ， 这 只 是 说 贝 叶 
斯 网 络 表达 ”了 它们 ， 还 没有 说 a 到 贝 叶 斯 网 络 的 求解 。 事 实 上 ， 后 珊 优 
化 的 目标 ， 就 古 在 这 些 既 有 的 约束 之 上 ， 通 过 调整 贝 叶 斯 网 络 中 随机 变 
量 的 取 值 ， 使 整个 后 验 概率 达到 最 大 : 


{x,l}* = arg max (xo) II P (aglr; t) [| P as L5). (11.15) 


耳 到 现在 为 止 ， 这 和 我 们 在 图 优化 中 谈论 的 未 西 并 没有 太 大 区 列 ， 
我 们 只 是 把 变量 间 的 关系 喧 式 地 用 有 问 图 给 表达 出 来 而 已 。 这 样 一 个 由 


条 件 概 众 摘 述 的 贝 叶 斯 网 络 ， 己 经 可 以 使 用 概率 图 模型 中 的 算法 进行 求 
解 了 。 不 过 ， 进 一 步 观察 ， 我 们 发 现 最 大 后 验 概 率 由 许多 项 因 于 乘积 而 
成 ， 因 此 该 贝 叶 斯 网 络 义 可 以 转化 成 一 个 因子 图 ( Factor Graph) 。 


11.3.2 ”因子 图 


因子 图 是 一 种 无 同 图 ， 由 两 种 节点 组 成 : 表示 优化 变量 的 变量 节点 
， 以 及 表示 因子 的 因子 节点 。 如 果 我 们 把 图 11-5 表 示 成 因子 图 ， 束 可 男 
成 如 图 11-6 所 示 的 样子 。 





OOO 


图 11-6 ”以 因子 图 形式 表达 的 SLAM 过 程 示 意图 。 圆 较为 变量 节 扣 ， 方 块 为 因 了 于 节 氮 。 


在 图 11-6 中 ， 我 们 用 圆圈 表示 变量 节操 。 注 童 与 贝 叶 斯 网 络 不 同 ， 
这 里 的 变量 是 SLAM 中 待 优化 的 部 分 ， 即 相机 位 姿 x 和 路 标 ! ， 而 没有 观 
测 z 和 输入 u 因为 这 几 个 量 是 给 定 的 而 不 十 行 优化 的 。 与 之 相对 ， 
因子 市 点 包含 了 行 优化 变量 之 间 的 关系， 它们 来 自 运 动 方 程 和 观测 方 
程 。 

对 因子 图 的 优化 ， 融 是 调整 各 变量 的 值 ， 使 它们 的 因 了 于 之 乘积 最 大 
化 一 一 它 依 然 对 应 着 一 个 优化 问题 。 在 通常 的 做 法 中 ， 我 们 把 各 因子 的 
条 件 概率 取 融 斯 分 布 的 形式 。 考 虑 a 到 运动 方程 为 


p= f (2541, Wg) -T Ulp. (11.16) 


其 中 w ~N (GR, )« WAR, 《实际 上 xi， MPL CIR DIR SG 





P (25s [apa ) = JM (f TENE ük) , Ri) (11.17) 


辣 理 ， 对 于 观测 数据 ， 有 : 


P (Ens [Bs te) == E Ce) e. (11.18) 
其 中 Q, 为 观测 方程 噪声 项 的 协 方差。 


通过 假设 高 斯 分 布 ， 我 们 显 式 地 表达 了 因子 图 优化 的 目标 函数 。 和 
图 优化 一 样 ， 由 于 取 最 大 的 后 验 概 率 相 当 于 取 负 对 数 的 最 小 化 ， 所 以 因 
子 图 优化 也 对 应 着 一 个 和 第 6 讲 介绍 的 相似 的 最 小 二 乘 问 题 ， 我 们 可 以 
用 高 斯 牛顿 法 或 列 文 介 格 一 马 伶 尔 特 方法 求解 一 个 因 了 于 图 优化 问题 。 次 
似 于 图 优化 ， 我 们 还 可 以 使 用 单元 的 、 二 元 的 或 多 元 的 因 了 于。 这 主要 看 
它 和 儿 个 变量 节点 有 关 。 

图 11-7 古 一 个 更 实际 的 因子 图 的 例子 。 











图 11-7 更 加 实际 的 因子 图 。 添 加 了 回环 因子 〈 撒 部 中 央 〉 和 先 验 因子 (的 部 两 侧 〉。 


运动 方程 和 观 误 方程 作为 因子 存在 于 图 中 。 此 外 ， 我 们 可 能 对 祭 些 
相机 位 姿 具 有 先 验 信息 一 一 例如 一 辆 在 室外 行驶 的 无 人 和 车， 我 们 可 能 通 
WGPS{a SHE SH SPREE AILS, ALA AY ART IA EE ir ZS iis 
加 先 验 因子 。 因 为 可 能 表达 成 概率 分 布 的 信息 有 许多 种 ， 于 是 因子 图 也 
可 以 定义 许多 不 同 的 因子 ， 比 如 ， 轮 式 编 码 右 的 测量 、IMU 的 测量 ， 等 





等 ， 使 之 成 为 一 种 非常 通用 的 优化 方式 。 
11.3.3” 增 量 特 性 


然而 ， 到 现在 为 止 ， 从 优化 角度 来 看 ， 优 化 一 个 因子 图 和 普通 的 图 
优化 并 没有 太 大 的 区 列 因为 我 们 最 后 面 对 的 都 是 一 个 最 小 二 乘 问 
cl, AMHR, EA pear PE. APA TE Al it 
MAW, RATE WH QR AE. Schurfh2kCholesky47 8€, JEXT 
ALS ALIA. MARIER: DT EUM n eR EL 
化 换 了 种 说 法 呢 ? 事情 并 不 完全 是 这 样 。 

Kaess 等 人 提出 的 SAM (incremental Smooth and Mapping) [Bl rp, 
对 因子 图 进行 了 更 加 精细 的 处 理 ， 使 得 它 可 以 增 量 式 Hh Ab FE Jr Pm DC 
化 。 回 忆 第 6 讲 的 内 容 ， 我 们 知道 ， 在 普通 的 图 优化 中 ， 最 终 要 计算 的 
是 一 个 增 量 陈 方程 。 即 如 何 调 整 目标 函数 





J(z) 2 Se}. Ry eot) X elk; keys; (11.19) 
k k 


中 的 优化 变量 ， 使 目标 函数 下 降 。 无 论 采 用 哪 种 梯度 下 降 策略 ， 最 
后 我 们 将 磁 到 一 个 形 如 


HAz = g. (11.20) 


HY Ae MET RE i SERA LE OE, RIITA SAA EP EY ait PE 
加 速 它 的 求解 过 程 。 然 而 ， 考 虑 到 图 优化 并 不 古 固定 的 ， 当 机 桌 人 运动 
时 ， 狐 的 市 点 和 边 将 被 加 入 图 中 ， 使 得 它 的 规模 不 断 增长 。 那 么 问题 来 
T: BERS TH, EA TT eT A BT E 
Ne? (包括 雅 可 比 的 求 取 (或 称 线性 化 ) 和 更 新 方程 的 计算 。) 


显然 ， 这 样 做 是 不 经 鹿 的， 我 们 以 图 11-8 为 例 来 解释 增 量 更 新 的 情 
况 。 这 是 一 个 位 姿 图 。 妆 我 们 控 照 里 程 计 方式 癌 其 中 添加 市 点 时 ， 受 影 
员 的 证 太 可 以 近似 地 看 成 只 有 最 后 一 个 与 之 相连 的 太后 ”， 而 早先 市 所 
的 估计 值 ， 可 以 近似 地 看 成 没有 友 生 变化 。 因 此 束 没 有 必要 对 它们 进行 
优化 。 为 什么 要 说 近似 呢 ? 因 为 实际 上 新 增 市 点 还 是 会 对 之 前 的 估计 产 


SMA, Arex mR age Ae A, BOLT AMR, BY LA 
忽略 挥 。 如 末 认 为 增 量 特性 可 行 ， 那 么 这 件 事 情 可 以 为 我 们 符 反 大 量 的 
计算 一 一 全 少 我 们 不 必 在 每 次 新 增 市 点 时 ， 对 整个 图 ”进行 优化 。 不 
过 ， 如 来 按照 回环 检 疯 的 方式 洪 加 节操 ， 那 么 受 影 啊 的 范围 应 该 是 回环 
JT 3538] 4 Bj WU — Ex HB APR s, ULL EBC BE n] He 5c T Us] 
整 。 这 虽然 加 大 了 计算 量 ， 然 而 我 们 依然 无 须 优 化 整 张 图 ， 因 为 回环 之 
JT RAED SCHEMA © Se D BD. RIRI, FERIA PRS RY, 
退 过 分 析 受 影响 的 区 域 ， 可 以 直观 上 ) 减少 一 些 没 必要 的 计算 ， 加 速 
后 响 优 化 流程 。 


oa 


Q 新 加 的 节点 
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图 11-8 ” 增 量 更 新 示意 图 。 


在 此 思想 的 基础 上 ，Kaess 等 人 提出 的 增 量 式 因 子 图 优化 一 定 程度 
上 解决 了 上 面 的 问题 。 从 技术 层面 上 来 看 ， 我 们 希望 在 每 次 优化 中 保存 
一 些 中 间 结 果 ; 而 当 新 的 变量 和 因子 加 入 时 ， 首 先 分 析 它 们 因子 图 之 间 
的 连接 和 影响 关系 ， 考 虑 之 前 存储 的 信息 有 哪些 可 以 继续 利用 ， 哪 些 必 
须 重 新 计算 ， 最 后 处 理 对 增 量 的 优化 。 话 虽 如 此 ， 由 于 具体 的 操作 步 又 
需要 介绍 大 量 的 技术 细节 ， 且 替 涉 到 了 概率 图 理论 的 知识 ， 超 出 了 本 书 
的 讨论 范围 。 所 以 本 节 藉 作为 可 选 讽 读 材 料 提 供给 读者 。 有 兴趣 的 读者 
可 以 阅读 ISAM、iSAM2 等 原始 论文 来 了 解 它 的 细节 处 理 。 最 后 ， 尺 官 
有 增 量 分 析 ， 但 我 们 必须 清楚 这 里 的 受 影 啊 节 点 亦 是 近似 的 ， 所 以 在 实 
际 操作 中 ， 当 图 的 规模 发 生 一 定 程 上 度 的 改变 时 ， 我 们 需要 再 做 一 次 全 图 
优化 。 


11.4 *#* 实 践 : gtsam 


11.4.1 %gtsam 4.0 


下 和 面 ， 我 们 演示 一 个 用 因子 图 进行 位 姿 图 优化 的 例子 。 我 们 仍 使 用 
前 面 “ 球 ” 的 数据 ， 不 同 的 是 ， 这 人 次 用 因子 图 来 优化 它 ， 而 不 是 g20 中 扑 
位 次 图 。 我 们 将 使 用 Gtsam®”7 ， 它 是 一 个 基于 因子 图 优化 的 SLAM 后 姗 
亩 ， 理 论 来 日 文献 [83,84]。 我 们 将 在 它 的 基础 上 ， 对 上 市 “ 球 ” 的 例子 进 
行 优化 。 





1 | git clone https://bitbucket.org/gtborg/gtsam. git 


gtsam 最 新 版 本 为 4.0， 位 于 https://bitbucket.org/gtborg/gtsam。 你 可 
以 输入 以 下 命令 下 载 它 : 由 于 gtsam 比 较 大 ， 我 们 没有 在 3rdparty 文 件 夹 
中 提供 。 与 以 往 壳 到 的 库 一 样 ，gtsam 亦 古 一 个 cmake 工 程 ， 我 们 按照 
cmake 的 方式 来 编译 安装 它 。gtsam 的 依赖 项 比较 少 ， 主 要 是 Eigen 和 和 tbb 
库 。 如 末 读 者 跟 独 本 书 一 路 走 来 ， 那 么 大 多 数 库 应 该 已 经 安 儿 好 了 ， 只 
需 安 装 tbb 库 即 可 : 


1 | sudo apt-get install libtbb-dev 


然后 , 使 用 cmake 命 令 对 gtsam 进 行 编 详 安 装 ， IX HAAN FEI 
gtam E LEEK, RK SITS SAFAHMAMWAA, EERIE RE 
ANDES. MURMER RR, LAM LEAF. BASE RK 
Ja, BJ 0) 4E/usr/local/include Fk $13 v fF, FE/usr/local/lib F $K BI) xc 
件 。gtsam 的 库 文 件 比较 简单 ， 仅 由 一 个 libgtsam.so 组 成 。 所 以 你 应 该 知 
道 如 何在 目 己 的 工程 中 书写 CMakeLists.txt 来 调用 gtsam 了 吧 ? 


11.42 MZ KIRE 


下 面 我 们 来 沽 示 “ 球 ”的 例子 。 与 前 面 的 实验 一 样 ， 


我 们 从 


sphere.g20 文 件 中 读 取 节点 和 边 的 信息 ， 转 换 为 因子 图 交 给 gtsam 处 理 ， 
然后 ， 把 优化 结果 重 狐 写 到 g20 文 件 ， 并 “ 伪 钱 ”成 g20 的 节点 和 边 以 便 显 
示 。 虽 然 这 似乎 没有 体现 出 gtsam 的 “ 增 量 ” 特 性 ， 但 人 至少 我们 可 以 通过 


这 个 例子 体验 一 
四 slambook/ch1 1/pose_graph_gtsam.cpp 


#include 
#include 
#include 


#include 


#include 


#include 


#include 
#include 
#include 
#include 


#include 


下 它 的 用 法 。 


<iostream> 
<fstream> 
<string> 


<Eigen/Core> 


<sophus/se3.h> 


<sophus/so3.h> 


<gtsam/slam/dataset.h> 
<gtsam/slam/BetweenFactor.h> 
<gtsam/slam/PriorFactor.h> 
«gtsam/nonlinear/GaussNewtonÜptimizer.h» 


<gtsam/nonlinear/LevenbergMarquardtOptimizer.h> 


using namespace std; 


using Sophus: 


using Sophus: 


SES" 
‘503% 


FEO IOI III I ICI I I TIT IR I I AIF 

* 本 程序 演示 如 何 用 gtsam 进行 位 姿 图 优化 

* sphere.g20 是 人 工 生 成 的 一 个 Pose graph， 我 们 来 优化 它 。 
* 与 g2o 相似 ， 在 gtsam 中 添加 的 是 因子 ， 相 当 于 误差 。 


* AIO GIGI IG IG IO GR EE EEE 


int main ( int argc, 


{ 


char** argv ) 


if ( argc != 2) 


1 


cout««"Usage: pose graph gtsam sphere.g2o"««endl; 


return 1; 


} 


ifstream fin ( argv[i] ); 


if £ 
{ 


!fin ) 


cout<<"file "<<argv[1]<<" does not exist."««endl; 


return 1; 


gtsam::NonlinearFactorGraph::shared ptr graph ( new gtsam::NonlinearFactorGraph 


// gtsam 的 因子 图 


Ji 


gtsam::Values::shared ptr initial ( new gtsam::Values ); // 初始 值 
// 从 g2o 文 件 中 读 取 节 点 和 边 的 信息 
int cntVertex=0, cntEdge = 0; 


cout<<"reading from g20 file"««endl; 


while ( !fin.eof() ) 
{ 
string tag; 
fin>>tag; 
if ( tag == "VERTEX_SE3:QUAT" ) 
{ 
// 顶点 
gtsam::Key id; 
fin>>id; 


double data[7] ; 
for ( int i=0; i<7; i++ ) fin>>data[i]; 


// 转换 至 gtsam 的 Pose3 


gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], 


data[5] ); 
gtsam::Point3 t ( data[0] data[1], data[2] ); 
initial->insert ( id, gtsam::Pose3 ( R,t ) ); 


cntVertex-t*; 
} 
else if ( tag == "EDGE_SE3:QUAT" ) 
{ 


// 边 ， 对 应 到 因子 图 中 的 因子 

gtsam::Matrix m = gtsam::I 6x6; // 信息 和 矩阵 
gtsam::Key idi, id2; 

fin>>id1i>>id2; 

double data[7]; 


for ( int i20; i«7; i++ ) fin>>data[i] ; 


// 添加 初始 值 


70 


105 


gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], 
data[5] ); 

gtsam::Point3 t ( data[0], data[1], data[2] ); 

for ( int i=0; i<6; i++ ) 


for ( int j=i; j<6; j++ ) 


{ 
double mij; 
fin>>mij; 
m (i,j ) = mij; 
m ( j,i) = mij; 
} 


// g2o Wits BFE RL AKA gtsam 不 同 ， 这 里 对 它 进 行 修 改 
gtsam::Matrix mgtsam = gtsam::I 6x6; 

mgtsam.block<3,3> ( 0,0 ) = m.block<3,3> ( 3,3 ); // cov rotation 
mgtsam.block<3,3> ( 3,3 ) m.block«3,3» ( 0,0 ); // cov translation 
mgtsam.block<3,3> ( 0,3 ) m.block<3,3> ( 0,3 ); // off diagonal 
mgtsam.block<3,3> ( 3,0 ) = m.block<3,3> ( 3,0 ); // off diagonal 


// 高 斯 噪声 模型 
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian:: 
Information ( mgtsam ); 
gtsam::NonlinearFactor::shared ptr factor ( 
new gtsam::BetweenFactor<gtsam::Pose3> ( idi, id2, gtsam::Pose3 ( R 


,t ), model ) // 添加 一 个 因子 


); 
graph-»push back ( factor ); 
cntEdget+; 
} 
if ( !fin.good() ) 
break; 


cout<<"read total "<<cntVertex<<" vertices, "<<cntEdge<<" edges."««endl; 
// 国定 第 一 个 顶点 ， 在 gtsam 中 相当 于 添加 一 个 先 验 因 子 
gtsam::NonlinearFactorGraph graphWithPrior = *graph; 


gtsam::noiseModel::Diagonal::shared ptr priorModel - 


gtsam::noiseModel::Diagonal::Variances ( 


)5 


( gtsam::Vector ( 6 ) ««1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6 ).finished() 


gtsam::Key firstKey - O0; 


for ( const gtsam::Values::ConstKeyValuePair& key value: *initial ) 


1 


cout<<"Adding prior to g2o file "««endl; 
graphWithPrior.add ( gtsam::PriorFactor<gtsam::Pose3> ( 


key value.key, key value.value.cast«gtsam::Pose3»(), priorModel ) 
) ; 


break; 


// 开始 因子 图 优化 ， 配 置 优 化 选项 

cout<<"optimizing the factor graph"««endl; 

// 我 们 使 用 列 文 伯 格 一 马 硅 尔 特 方法 优化 

gtsam::LevenbergMarquardtParams params lm; 

params lm.setVerbosity ("ERROR"); 

params lm.setMaxIterations(20); 

params lm.setLinearSolverType("MULTIFRONTAL QR"); 
gtsam::LevenbergMarquardtÜptimizer optimizer LM( graphWithPrior, *initial, 


params lm ); 


// 你 可 以 尝试 下 高 斯 牛顿 法 

// gtsam::GaussNewtonParams params gn; 

// params gn.setVerbosity("ERROR"); 

// params_gn.setMarIterations (20) ; 

// params gn.setLinearSolverType("MULTIFRONTAL QR"); 

// gtsam::GaussNeutonÜptimizer optimizer ( graphWithPrior, *initial, params gn 


2; 


gtsam::Values result = optimizer LM.optimize(); 
cout««"Üptimization complete"««endl; 
cout<<"initial error: "<<graph->error ( *initial ) ««endl; 


cout<<"final error: "<<graph->error ( result ) ««endl; 


cout<<"done. write to g20 ... "««endl; 
// BX g2o 文件 ， 同 样 伪装 成 g2o 中 的 顶点 和 边 ， 以 便 用 g2o viewer 查看 
// 顶点 
ofstream fout ( "result_gtsam.g20" ); 
for ( const gtsam::Values::ConstKeyValuePair& key value: result ) 
1 
gtsam::Pose3 pose = key value.value.cast«gtsam::Pose3»(); 
gtsam::Point3 p = pose.translation(); 
gtsam::Quaternion q = pose.rotation().toQuaternion(); 
fout««"VERTEX SES3:QUAT "««key value.key««" " 
€«p.x(O <<" "“<<p.gQ <<" "««p.z() <<" " 
«X«q.x()««" "««q.y()««" "««q.z()««" "<<q.wO <<" "««endl1; 


} 

// ih 

for ( gtsam::NonlinearFactor::shared, ptr factor: *graph ) 
1 


gtsam::BetweenFactor«gtsam::Pose3»::shared ptr f = dynamic pointer, cast« 


156 


184 


185 


gtsam: :BetweenFactor<gtsam: :Pose3>>( factor ); 
if ( f) 
1 
gtsam::SharedNoiseModel model = f-»noiseModel(); 
gtsam::noiseModel::Gaussian::shared ptr gaussianModel - 
dynamic, pointer cast«gtsam::noiseModel::Gaussian»( model ); 
if ( gaussianModel ) 
{ 
// write the edge information 
gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel 
“RQ 
gtsam: :Pose3 pose = f->measured() ; 
gtsam::Point3 p = pose.translation(); 
gtsam::Quaternion q = pose.rotation().toQuaternion(); 
fout<<"EDGE_SE3:QUAT "««f-»key1()««" "««f-»key2()««" " 
CAD RY) <<" Negy <<" “<<p.2zQ <<" " 
<<q.x()<<" "««q.y()««" "««q.z(««" "««q.w()««" "; 
gtsam::Matrix infoG2o = gtsam::I, 6x6; 
infoG20.block(0,0,3,3) = info.block(3,3,3,3); // cov translation 
infoG20.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation 
infoG20.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal 
infoG20.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal 


for ( int i=0; i<6; i++ ) 


for ( int j=i; j<6; j++ ) 
{ 

fout««infoG2o(i,j)««" "; 
} 


fout««endl; 


) 


fout.close(); 


cout««"done."««endl; 


在 泪 示 程序 中 ， 我 们 以 文本 方式 读 取 一 个 g2o 文 件 ， 并 完成 了 回 


gtsam 接 口 的 转换 。 请 留意 代码 中 g20 与 gtsam 的 异同 点 。 比 如 说 相同 的 


地 方 : 


1. 由 于 它们 本 质 上 都 是 同一 个 最 小 二 乘 优 化 问题 ， 所 以 我 们 要 设置 
的 东西 也 是 类 似 的 。 们 而 言 之 ， 即 顶点 《对 应 到 因子 图 中 的 变量 ) BUS 
值 ， 以 及 边 〈 对 应 到 因子 ) 的 观测 值 ， 还 有 噪声 大 小 。 

2. 在 优化 设置 方面 ， 同 样 可 以 使 用 高 斯 牛顿 法 或 列 文 介 格 一 马 仿 尔 
特 方法 ， 并 配置 详细 的 参数 ， 只 不 过 接口 略 有 不 同 。g2o 通 过 构建 优化 
算法 对 象 来 实现 ， 而 gtsam 则 是 传 入 优化 参数 。 

不 同 的 地 方 : 

1.g20 的 噪声 模型 和 gtsam 稍 有 个 同 ， 我 们 在 代码 中 做 了 一 下 转换 。 

2. 在 稀 惑 化 的 处 理 方 面 ，gtsam 可 选择 使 用 QR 分 解 或 Cholesky 分 
解 ， 尽 管 我 们 没有 详细 解释 这 方面 的 具体 过 程 。 读 者 可 以 退 踩 参数 的 配 
置 方式 ， 看 看 gtsam 提 供 哪 些 求解 方法 。 由 于 单纯 的 Pose Graphic I € 
稀 玩 性 可 以 利用 ， 所 以 这 里 区 别 不 大 。 

最 后 ， 我 们 把 gtsam 的 优化 结果 转换 为 g2o 的 输出 文件 ， 同 样 可 以 用 
g20_viewer 全 看 此 结果 ， 如 图 11-9 所 示 。 





Optimizer 
gn_var_cholmod 


Parameters 


Spanning Tree 
Initial Guess 
SetZero 


Optimize loaded result gtsam.g20 with 2500 vertices and 9799 measuremen ts 
graph is fixed by node 2499 


图 11-9 gtsam 的 优化 结 
下 面 是 终 新 输出 的 优化 信息 : 





1 |$ build/pose_graph_gtsam sphere.g20 

2 |reading from g20 file 

3 |read total 2500 vertices, 9799 edges. 

4 | Adding prior to g2o file 

s |optimizing the factor graph 

6 | Initial error: 4.7724e+09 

7 |newError: 6.07118e+08 

8 |errorThreshold: 6.07118e+08 > 0 

9 | absoluteDecrease: 4165284178.46 >= 1e-05 
10 | relativeDecrease: 0.872785675288 >= 1e-05 
u | // 中 间 略 

12 |newError: 63793.9289983 


13 | errorThreshold: 63793.9289983 > 0 

14 | absoluteDecrease: 0.279685092828 >= 1e-05 

15 |relativeDecrease: 4.38417684928e-06 « 1e-05 
16 | converged 

17 | errorThreshold: 63793.9289983 <? 0 

18 | absoluteDecrease: 0.279685092828 <? 1e-05 

19 |relativeDecrease: 4.38417684928e-06 <? 1e-05 
20 | iterations: 5 >? 20 

21 | Optimization complete 

2 |initial error: 4772402087.24 

23 | final error: 63793.9289982 

24 | done. write to g2o ... 


25 | done. 
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后 算法 就 收 你 了 。 同 样 ， 由 于 gtsam 的 误差 定义 方式 与 g2o 不 同 ， 所 以 这 
里 直接 比较 误 友 大 小 没有 太 大 音义。 如 末 用 g20_viewer 打 开 
result_gtsam.g20 文 件 并 选择 优化 ， 束 会 发 现在 g20 上 度量 下 的 误差 大 约 为 
44360 左 右 ， 和 我 们 上 一 厄 使 用 至 代数 优化 的 结果 一 怪 ， 但 gtsam 的 从 代 
PRB BE > — ES 


有 点 遗憾 的 是 ， 本 例 没 有 体现 出 gtsam 的 增 量 特性 。 如 果 我 们 把 g2o 
文件 中 的 节点 和 边 按 照 时 间 排 序 ， 然 后 一 个 一 个 地 放 入 gtsam 中 ， 可 能 
对 它 的 增 量 特征 会 有 更 好 的 表述 。 我 们 把 这 个 实验 留 作 习 题 ， 交 给 读者 

习题 

1. 如 果 将 位 姿 图 中 的 误差 定 义 为 Atr =6067', 推导 按照 此 定义 的 左 
HELA HE n] EG FEM 

2. 参 照 g2o 的 程序 ， 在 Ceres 中 实现 对 “ 球 ” 位 姿 图 的 优化 。 

3. 对 “ 球 ” 中 的 信息 按照 时 间 排 序 ， 分 别 喂 给 g2o0 和 gtsam 人 优化， 比较 
' 1B TEBE ZAR. 

4.* 阅 读 ISAM 相 关 论 文 ， 理 解 它 是 如 何 实 现 增 量 式 优化 的 。 





[1] 请 注意 ， 尽 管 数 值 上 看 此 处 的 误 状 要 大 一 些 ， 但 是 由 于 我 们 目 定义 边 时 重新 定义 了 误 兰 的 计 
算 方 式 ， 所 以 此 处 数值 的 大 小 并 不 能 直接 用 于 比较 。 


[2] 由 于 没有 做 更 多 的 实验 ， 所 以 该 结论 只 在 “ 球 ” 这 个 例子 上 有 效 。 
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主要 目标 

1. 理 解 回环 检测 的 必要 性 。 

2. 掌 握 基 于 词 袋 的 外 观 式 回环 检测 。 

3. 通 过 DBoW3 的 实验 ， 学 习 词 袋 模 型 的 实际 用 途 。 

本 讲 中 ， 我 们 来 介绍 SLAM 中 男 一 个 主要 模块 回环 检测 。 我 们 知 
道 SLAM 主 体 〈 前 端 、 后 端 ) 主要 的 目的 在 于 估计 相机 运动 ， 而 回环 检 
测 模 块 ， 无 论 是 目标 上 还 是 方法 上 ， 都 与 前 面 讲 的 内 容 相 差 较 大 ， 上 所 以 
通 稼 被 认为 是 一 个 独立 的 模块 。 我 们 将 介绍 主流 视觉 SLAM 中 检测 回环 
的 方式 : 词 袋 模型 ， 并 通过 DBoW 库 上 的 程序 实验 ， 使 读者 得 到 更 加 直 
观 的 理解 。 


12.1 回环 检测 概述 


12.1.1 回环 检测 的 意义 


我 们 已 然 介 绍 了 前 端 和 后 端 ， 前 端 提 供 特征 点 的 提取 和 轨迹 、 地 图 
的 初 值 ， 而 后 端 负责 对 所 有 这 些 数据 进行 优化 。 然 而 ， 如 果 像 VO 那 样 
仅 考虑 相 邻 时 间 上 的 关联 ， 那 么 ， 之 前 产生 的 误差 将 不 可 避免 地 累积 到 
下 一 个 时 刻 ， 使 得 整个 SLAM 会 出 现 累积 误差 ， 长 期 估计 的 结果 将 不 可 
靠 ， 或 者 说 ， 我 们 无 法 构建 全 局 一 致 的 轨迹 和 地 图 。 

举例 来 说 ， 假 设 我 们 在 前 端 提 取 了 特征 ， 然 后 忽略 掉 特 征 点 ， 在 后 
端 使 用 poseGraph 优 化 整个 轨迹 ， 如 图 12-1(a) 所 示 。 由 于 前 端 给 出 的 只 
是 局 部 的 位 姿 间 约束 ， 比 如 ， 可 能 是 x , -x , x. -x。， 等 等 。 但 是 ， 由 于 
估计 存在 误差 ， 而 x , 是 根据 x 决定 的 ，x , 又 是 由 x , 决定 的 。 以 此 类 推 ， 
误差 就 会 被 累积 起 来 ， 使 得 后 端 优化 的 结果 如 图 12-1(b) 所 示 ， 慢 慢 地 赵 
向 不 准确 。 


(al (b) (c) 


图 12-1 漂移 示意 图 。 (a) 真实 轨迹 ; b) 由 于 前 端 只 给 出 相 邻 帧 加 的 估计 ， 优 化 后 的 
Pose Graph EWI; Cc) 添加 回环 检测 后 的 Pose Graph 可 以 消除 累积 误差 。 


时 然后 是 能 够 估计 最 大 后 验 误 友 ,但 所 谓 “ 好 柑 型 染 不 住 粒 数据 ”， 


只 有 相 邻 天 键 帧 数据 时 ， 我 们 能 做 的 事情 并 不 很 多 ， 也 无 从 六 除 索 积 误 
下 。 但 是 ， 回 环 检 柚 模 块 能 够 给 出 除了 相 邻 帧 之 外 的 一 些 时 隔 更 加 入 远 


的 约束 : 例如 x | -xi 之 间 的 位 姿 变 换 。 为 什么 它们 之 间 会 有 约束 呢 ? 这 
征 因为 我 们 察觉 到 相机 经 过 了 同一 个 地 方 ， 采 集 到 了 相似 的 数据 。 而 
回环 检测 的 天 键 ， 束 是 如 何 有 效 地 检测 出 相机 经 过 同一 个 地 方 这 件 
事 。 如 果 我 们 能 够 成 功 地 检测 到 这 件 事 ， 束 可 以 为 后 问 的 Pose Graph 提 
供 更 多 的 有 效 数 据 ， 使 之 得 到 更 好 的 估计 ， 特 别 是 得 到 一 个 全 局 一 致 

(Global Consistent) 的 估计 。 由 于 Pose Graph 可 以 看 成 一 个 质点 一 一 弹 
BAS, POE ARMA TEAR PULA SAPS, fer SA 
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确 的 位 置 一 一 如 有 果 回 坏 本 号 正确 的 话 。 


回环 检测 对 于 SLAM 系 统 意 义 重 大 。 它 关系 到 我 们 估计 的 轨迹 和 地 
图 在 长 时 间 下 ”的 正确 性 。 男 一 方面 ， 由 于 回环 检测 提供 了 当前 数据 与 
所 有 历史 数据 的 关联 ， 在 跟踪 算法 丢失 之 后 ， 我 们 还 可 以 利用 回环 检测 
进行 重 定 位 。 因 此 ， 回 环 检测 对 整个 SLAM 系 统 精度 与 稳健 性 的 提升 是 
非常 明显 的 。 其 至 在 某 些 时 候 ， 我 们 把 仪 有 前 端 和 局 部 后 端的 系统 称 为 
VO， 而 把 带 有 回环 检测 和 全 局 后 端的 系统 称 为 SLAM。 


12.1.2 方法 


下 面 我 们 来 考虑 回环 检 疯 如 何 实现 的 问题 。 

最 傈 单 的 方式 就 是 对 任意 两 幅 图 像 部 做 一 表 特 征 罗 配 ， 根 据 正确 下 
配 的 数量 确定 哪 两 幅 图 像 存 在 关联 一 一 这 确实 是 一 种 朴 妈 且 有 效 的 思 
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得 要 检 训 的 数量 实在 太 大 : 对 于 个 可 能 的 回环 ， 我 们 要 检 训 CC” y 那么 


EWR, ZEO (N? ) 的 复 森 度 ， 随 看 轨迹 变 长 增长 太 快 ， 在 大 多 数 实时 系 
统 当 中 是 不 实用 的 。 故 一 种 杆 系 的 方式 是 ， 随 机 抽取 历史 数据 并 进行 回 
环 检 测 ， 比 如 说 在 n ” 帧 当中 随机 抽 5 帧 与 当前 帧 比较 。 这 种 做 法 能 够 维 
持 单 数 时 间 的 运算 量 ， 但 是 这 种 育 目 试探 方法 在 帧 数 N 增长 时 ， 抽 到 回 
环 的 几率 义 大 幅 下 降 ， 使 得 检测 效率 不 遍 。 

上 和 面 说 的 杆 系 思路 部 过 于 粗粮 。 尺 宦 随 机 检测 在 有 些 实现 中 确实 有 有 
用 又 ， 但 我 们 至 少 希 望 有 一 个 “ 哪 处 可 能 出 现 回环 ”的 预 计 ， 才 好 不 那 
么 吾 目 地 去 检测 。 这 样 的 方式 大 体 分 为 两 种 筷 路 : 基于 里 程 计 的 几何 天 


系 COdometry based) ， 或 基于 外 观 (Appearance based) 。 基 于 几何 关 
系 是 说 ， 当 我 们 有 发现 当前 相机 运动 到 了 之 前 的 茶 个 位 置 附近 时 ， 检 测 筷 
们 有 没有 回环 关系 时 ”一 一 这 目 然 是 一 种 直观 的 想法 ， 但 是 由 于 索 积 误 
才 的 存在 ， 我 们 往往 没 法 正确 地 发 现 “ 运 动 到 了 之 前 的 菜 个 位 置 附近 ”这 
件 事实 ， 回 环 检测 也 无 从 谈 起 。 因 此 ， 这 种 做 法 在 逻辑 上 存在 一 点 问 
里 ， 因 为 回环 检测 的 目标 在 于 有 发现“ 相机 回 到 之 前 位 置 ?的 事实 ， 从 而 消 
除 昧 积 误 竺 。 而 基于 几何 关系 的 做 法 假设 了 “相机 回 到 之 前 位 置 附近 ” 
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有 效 地 在 不 同 场景 下 工作 ， 成 为 了 视觉 SLAM 中 主流 的 做 法 ， 并 人 被 应 用 
于 实际 的 系统 中 去 多 和 0 人 


在 基于 外 观 的 回环 检 剖 算法 中 ， 核 心 问题 是 如 何 计算 图 像 间 的 相似 
性 o Cuan, STARA 和 图 像 B ， 我 们 要 设计 一 种 方法 ， 计 算 它 们 之 间 
的 相似 性 评分 : s (A,B )。 当 然 这 个 评分 会 在 菏 个 区 则 内 取 什 ， 当 它 大 于 
一 定量 后 我 们 认为 出 现 了 一 个 回环 。 读 者 可 能 会 有 疑问 : 计算 两 幅 图 像 
之 间 的 相似 性 很 困难 吗 ? 例如 吾 观 上 看 ， 图 像 能 够 表示 成 算 阵 ， 那 么 直 
接 让 两 幅 图 像 相 减 ， 然 后 取 东 种 范 数 行 不 行 呢 ? 


s(A, B) = ||A — B. (12.1) 


为 什么 我 们 不 这 样 做 ? 

1. 首 先 ， 前 面 也 说 过 ， 像 系 灰 度 是 一 种 不 稳定 的 测量 值 ， 它 严重 地 
受 环 境 光 照 和 相机 轻 区 的 影响 。 假 变相 机 未 动 ， 我 们 打开 了 一 文 电灯 ， 
那么 图 像 会 整体 变 亮 一 些 。 这 样 ， 即 使 对 于 同样 的 数据 ， 我 们 都 会 得 到 
一 个 很 大 的 差异 值 。 

2. 刃 一 方面 ， 当 相机 视角 友 生 少量 变化 时 ， 即 使 每 个 物体 的 光度 不 
变 ， 它 们 的 像 系 也 会 在 图 像 中 有 友 生 位 移 ， 造 成 一 个 很 大 的 开 异 值 。 

由 于 这 两 种 情况 的 存在 ， 实 际 当 中 ， 即 使 对 于 非 第 相似 的 图 像 ，A- 


B 也 会 经 名 得 到 一 个 《不 符合 实际 的 ) 很 大 的 值 。 所 以 我 们 说 ， 这 个 函 
数 不 能 很 好 地 反映 图 像 间 的 相似 关系 。 这 里 罕 涉 到 一 个 “好 ”和 “不 
好 ”的 定义 问题 。 我 们 要 问 ， 怎 样 的 函数 能 人 够 更 好 地 反映 相似 关系 ， 而 

怎样 的 图 数 不 够 好 呢 ? Max Ru EAS d EAS (Perceptual Aliasing) 

和 感知 变异 (PerceptualVariability) 两 个 概念 。 现 在 我 们 来 更 详细 地 讨 
从 全 下。 


12.1.3 “ERA AG EK 


MARWARE, Ces vAd 我 们 能 够 以 很 高 的 精确 度 ， 
感觉 到 “两 幅 图 像 是 合 相 似 ? 臣 “这 两 张 照 刻 是 从 同一 个 地 方 拍摄 的 "> 这 一 
事实 ， 但 由 于 目前 尚未 掌握 人 脑 的 工作 原理 ， 我 们 无 法 清楚 地 描述 自己 
是 如 何 完成 这 件 事 的 。 从 程序 角度 看 ， 我 们 布 望 程序 算法 能 够 得 出 和 人 
类 ， 或 者 和 事实 一 任 的 判断 。 当 我 们 党 得， 或 者 事实 上 就 是 ， 两 幅 图 像 
从 同一 个 地 方 拍摄 ， 那 么 回环 检测 算法 也 应 该 给 出 “这 是 回环 ”的 绩 打 。 
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么 程序 也 应 该 给 出 “这 不 是 回环 ”的 判断 。 叫 32A. REPRE ADT SEAS he 
与 我 们 人 类 的 想法 一 致 ， 所 以 可 能 出 现 表 12-1 中 的 4 种 情况 : 


表 12-1 回 环 检测 的 结 采 分 类 








个 征 回环 
假 阳 性 ( False Positive ) 
真 阴 性 〈True Negative ) 





这 里 阴性 /阳性 的 说 法 是 借用 了 医学 上 的 说 法 。 假 阳性 (False 
Positive) 又 称 为 感知 人 往 和 震 ， 而 假 阴 性 (False Negative) 称 为 感知 变异 
( 见 图 12-2) 。 为 方便 书写 ， 用 缩写 TP 代 表 True Positive, HAH. 
由 于 我 们 希望 算法 和 人 类 的 判断 一 致 ， 所 以 布 望 TP 和 TN 要 尽量 高 ， 而 
FP 和 FN 要 尺 可 能 低 。 所 以 ， 对 于 菜 种 特定 算法 ， 我 们 可 以 统计 它 在 某 
个 数据 集 上 的 TP、TN、FP、FN 的 出 现 次 数 ， 并 计算 两 个 统计 量 : 准确 
A 和 召回 率 (Precision&Recall) 。 


Precision = TP/(TP+ FP), Recall = TP/(TP-+ FN). [13.2] 





False Positive False Negative 


图 12-2 BIESSE. AMEE, PAIS RE ERRA, THO EIE] —ÀE 
JB: AEUDAMBUHTE. BTR, [8]—38 5x A [EJIST ZI RU Fr ERRADAS. 


WASP HR XY EORA, TEMA ce, BUSTER BIAIS 
确实 是 其 实 回 环 的 概率 。 而 召回 率 则 是 说 ， 在 所 有 其 实 回 环 中 被 正确 检 
测 出 来 的 概率 。 为 什么 取 这 两 个 统计 量 呢 ? 因 为 它们 有 一 定 的 代表 性 ， 
JF Ha ORE I P J8 。 

一 个 算法 往往 有 许多 的 设置 参数 。 比 如 说 ， 当 提高 某 个 国 值 时 ， 算 
法 可 能 变 得 更 加 “严格 ”一 一 它 检 出 更 少 的 回环 ， 使 准确 率 得 以 提高 。 但 
同时 ， 由 于 检 出 的 数量 变 少 了 ， 许 多 原本 是 回环 的 地 方丈 可 能 补漏 挥 
了 了， 叶 插 召回 紊 下 降 。 有 反之， 如 来 我 们 选择 更 加 宽松 的 配置 ， 那 么 检 出 
的 回环 数量 将 增加 ， 得 到 更 高 的 人 各 回 率 ， 但 其 中 可 能 混杂 一 些 不 是 回环 
的 情况 ， 于 十 准确 率 下 降 。 


为 了 评价 算法 的 好 坏 ， 我 们 会 测试 它 在 各 种 配置 下 的 P FAIR f. TA 
后 做 出 一 条 Precision-Recall 曲 线 〈 见 图 12-3) 。 当 用 如 回 率 为 椰 轴 ， 用 
准确 率 为 纵 轴 时 ， 我 们 会 关心 整 条 曲线 侦 癌 右上 方 的 程度 、1009%6 准 确 
率 下 的 各 回 率 或 者 50% 召 回 率 时 的 准确 挛 ， 作 为 评价 算法 的 指标 。 不 过 
请 注意 ， 除 去 一 些 “ 天 后 之 列 ” 的 算法 ， 我 们 通 钊 不 能 一 概 而 论 地 说 算法 
A 惑 是 优 于 算法 B 的 。 我 们 可 能 会 说 A 在 准确 鞭 较 高 时 还 有 很 好 的 各 回 
率 ， 而 B 在 70% 和 召回 率 的 情况 下 还 能 保证 较 好 的 准确 著 ， 诺 如 此 类 。 


值得 一 提 的 是 ， 在 SLAM 中 ， 我 们 对 准确 率 要 求 更 局 ， 而 对 台 回 率 
则 相对 需 容 一 些 。 由 于 假 阳 性 的 (检测 结果 是 而 实际 不 是 的 ) 回环 将 在 
后 病 的 Pose Graph 中 洪 加 根本 错误 的 边 ， 有 些 时 候 会 叶 致 优化 算法 给 出 
完全 错误 的 结果 。 想 象 一 下 ， 如 果 SLAM 程 序 错误 地 将 所 有 的 办 公 桌 当 
成 了 同一 张 ， 那 建 出 来 的 图 会 怎么 梓 呢 ? RARESA NERDE S, Hi 
壁 被 区 钳 在 一 起 了 ， 了 最 后 整个 地 图 都 失效 了 。 而 相 比 之 下 ， 如 回 率 低 一 
些 ， 则 项 多 有 部 分 的 回环 没有 被 检测 到 ， 地 图 可 能 有 党 一 些 累 积 误 关 的 影 
困 一 一 然而 仅 需 一 两 次 回环 融 可 以 完全 消除 它们 了 。 所 以 说 和 在 选择 回环 








检测 算法 时 ， 我 们 更 倾 同 于 把 参数 设置 得 更 严格 一 些 ， 或 者 在 检测 之 后 
再 加 上 回环 验证 的 步 又 。 


Precision 
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fab-map 2.0 
0.9 do. d a ESTA, 





0 0:1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 
Recall 


图 12-3 “准确 率 -召回 率 曲线 的 例子 ea 。 随 着 召回 率 的 上 升 ， 检 测 条 件 变 得 宽松 ， 准 确 率 
随 之 下 降 。 好 的 算法 在 较 高 召回 率 情况 下 仍 能 保证 较 好 的 准确 率 。 


那么 ， 回 到 之 前 的 问题 ， 为 什么 不 用 A-B 来 计算 相似 性 呢 ? 我 们 会 


及 现 它 的 准确 等 和 各 回 率 都 很 甜 ， 可 能 出 现 大 量 的 False Positive 或 False 
Negative 的 情况 ， 所 以 说 这 样 做 “不 好 ”。 那 么 ， 什 么 方法 更 好 一 些 呢 ? 





12.2” 词 袋 模型 


既然 直接 用 两 张 图 像 相 减 的 方式 不 够 好 ， 那 么 我 们 就 需要 一 种 更 加 
可 菲 的 方式 。 结 合 前 面 几 讲 的 内 容 ， 一 种 直观 的 思路 是 : 为 何不 像 VO 
那样 使 用 特征 点 来 做 回环 检测 昵 ? 和 VO 一 样 ， 我 们 对 两 幅 图 像 的 特征 
点 进行 下 配 ， 只 要 区 配 数 量 大 于 一 定 值 ， 束 认为 出 现 了 了 回环。 进一步 ， 
根据 特征 点 匹配 ， 我 们 还 能 计 宽 出 这 两 幅 图 像 之 间 的 运动 关系 。 当 然 这 
种 做 法 会 存在 一 些 问 题 ， 例 如 ， 特 征 的 匹配 会 比较 费时 、 当 光照 变化 时 
特征 描述 可 能 不 稳定 等 ， 但 离 我 们 要 介绍 的 词 袋 模型 已 经 很 相近 了 。 下 
面 我 们 先 来 讲 词 袋 的 做 法 ， 再 来 讨论 数据 结构 之 类 的 实现 细节 。 

词 袋 ， 也 束 是 Bag-of-Words (Bow) ， 目 的 是 用 “图 像 上 有 哪 几 种 
PIL RSI AR. PIO, BATISTA PAN A AE; 
而 男 一 张 中 有 两 个 人 、 一 只 狗 。 根 据 这 样 的 接 述 ， 束 可 以 度量 这 两 幅 图 
像 的 相似 性 。 再 具体 一 些 ， 我 们 要 做 以 下 几 件 事 : 

1.08 xg * A" 2E») EET 对 应 于 BoW 中 的 “单词 ”(Word) ， 
许多 单词 放 在 一 起 ， 组 成 了 了“ 字典”(Dictionary) 。 

2. 俏 定 一 幅 图 像 中 出 现 了 哪些 在 字典 中 定义 的 概念 一 一 我 们 用 单词 
出 现 的 情况 (或 直方 图 ) 摘 述 整 幅 图 像 。 这 残 把 一 幅 图 像 转 换 成 了 一 个 
问 量 的 摘 述 。 

3. 比 较 上 一 步 中 的 描述 的 相似 程度 。 

以 上 面 举 的 例子 来 说， 首先 我 们 通过 条 种 方式 得 到 了 一 本 “字典 ”。 
字典 上 记录 了 许多 蛙 词 ， 每 个 单词 都 有 一 定 意义 ， 例如 “人 “车 ”“ 狗 ”都 
是 记录 在 字典 中 的 单词 ， 我 们 不 妨 记 为 w wow, Ala, HERA 
A， 根 据 它 们 含有 的 单词 ， 可 记 为 


4 一 1.1 十 1.o2 十 0.3. (12.3) 








字典 是 国定 的 ， 所 以 只 要 用 [1，1，0]r 这 个 向 量 就 可 以 表达 A 的 意 
义 。 通 过 字典 和 单词 ， 只 需 一 个 向 量 就 可 以 描述 整 幅 图 像 了 。 该 向 量 描 
述 的 是 “图 像 是 否 含有 某 类 特征 的 信息 ， 比 单纯 的 灰 度 值 更 加 稳定 。 又 


因为 摘 述 同 量 说 的 是 “是 个 出 现 ”， 而 不 管 它 们 "在 哪儿 出 现 ”， 上 所 以 与 
物体 的 空间 位 置 和 排列 顺序 无 关 ， 因 此 在 相机 用 生 少 量 运动 时 ， 只 要 物 
体 仍 在 视野 中 出 现 ， 我 们 就 仍然 保证 摘 述 同 量 不 发 生变 化 。 欠 基于 这 种 
特性 ， 我 们 称 它 为 Bag-of-Words 而 不 是 什么 List-of-Words， 强 调 的 是 

Words 的 有 无 ， 而 无 关 其 顺序 。 因 此 ， 可 以 说 字典 类 似 于 单词 的 一 个 集 


LS 
LJ o 


回 到 上 面 的 例子 ， 同 理 ， 用 [2，0，1]7 可 以 描述 图 像 B 。 如 果 只 考 
虑 “ 征 仿 出现” 而 不 考虑 数量 ， 也 可 以 是 [1 0, 1]T ， 这 时 候 这 个 同 量 就 是 
二 值 的 。 于 是 ， 根 据 这 两 个 向 量 ， 设 计 一 定 的 计算 方式 ， 束 能 确定 图 像 
间 的 相似 性 了 。 当 然 ， 如 果 对 两 个 同 量 求 大 仍然 有 一 些 不 同 的 做 法 ， 比 
如 对 于 q,be RW， 可 以 计算 : 


1 
s (a, b) = l 7, |la - bl . (12.4) 


其 中 范 数 取 L , 范 数 ， 即 各 元 素 绝对 值 之 和 。 请 注意 在 两 个 向 量 完全 
一 样 时 ， 我 们 将 得 到 1; 完全 相反 时 Ca 为 0 的 地 方 b 为 1) 得 到 0。 这 样 
就 定义 了 两 个 描述 问 量 的 相似 性 ， 也 束 定 义 了 图 像 之 间 的 相似 程度 。 

接 下 来 的 问题 是 什么 呢 ? 

1. 我 们 虽然 清楚 了 字典 的 定义 方式 ， 但 它 到 搬 是 怎么 来 的 呢 ? 

2. 如 果 我 们 能 够 计算 两 幅 图 像 间 的 相似 程度 评分 ， 是 否 束 足够 判断 
回环 了 呢 ? 


所 以 接 下 来 ， 我 们 首先 介绍 字典 的 生成 方式 ， 然 后 介绍 如 何 利用 字 
典 实际 地 计算 两 幅 图 像 间 的 相似 性 。 


12.3 ”字典 


12.3.1 字典 的 结构 


按照 前 面 的 介绍 ， 字 和 典 由 很 多 单词 组 成 ， 而 每 一 个 单词 代表 了 一 个 
概念 。 一 个 单词 与 一 个 单独 的 特征 点 不 同 ， 它 不 是 从 单 幅 图 像 上 提取 出 
来 的 ， 而 是 条 一 类 特征 的 组 合 。 所 以 ， 字 典 生 成 问题 类 似 于 一 个 聚 类 
(Clustering) 问题 。 

肾 类 问题 在 无 监督 机 妖 学 习 (Unsupervised ML) 中 特别 第 见 ， 用 
于 让 机 器 自行 寻找 数据 中 的 规律 。BoW 的 字 — 典 生成 问题 亦 属于 其 中 之 
一 。 首 先 ， 假 设 我 们 对 大 量 的 图 像 提 取 了 特征 点 ， 比 如 说 有 N 个 。 现 
E, RIER -TAk 个 单词 的 字典 ， 每 个 单词 可 以 看 作 局 部 相 邻 特征 
点 的 集合 ， 应 该 怎 么 做 呢 ? 这 可 以 用 经 典 的 K-means (KHA) YA. 
解决 。 

K-means 古 一 个 非常 答 单 有 效 的 方法 ， 因 此 在 无 监督 学 习 中 广 为 使 
用 ， 下 面 对 其 原理 稍 做 介绍 。 人 简单 的 说 ， 当 有 个 数据 ， 想 要 归 成 k 个 
类 ， 那 么 用 K-means 来 做 主要 包括 如 下 步骤 : 

1. 随 机 选取 k 个 中 心 点 : CC o 

2. 对 每 一 个 样本 ， 计 算 它 与 每 个 中 心 点 之 间 的 距离 ， 取 最 小 的 作为 
ERIX. 

3. 午 新 计算 每 个 类 的 中 心 点 。 

4. 如 果 每 个 中 心 点 都 变化 很 小 ， 则 算法 收敛 ， 退 出 ; 人 否则 返回 第 2 
步 。 

K-means 的 做 法 是 朴素 且 简单 有 效 的 ， 不 过 也 存在 一 些 问 题 ， 例 
如 ， 需 要 指定 肾 类 数量 、 随 机 选取 中 心 点 使 得 每 次 肾 类 结果 部 不 相同 ， 
以 及 一 些 效 识 上 的 问题 。 随 后 研究 者 们 亦 开 友 出 了 层 炊 聚 类 法 、K- 
meanst+) 等 算法 以 弥补 它 的 不 足 ， 不 过 这 都 是 后 话 ， 我 们 束 不 详细 讨 
论 了 。 总 之 ， 根 据 K-means， 我 们 可 以 把 已 经 提取 的 大 量 特征 点 聚 类 成 


一 个 含有 k 个 单词 的 字典 了 。 现 在 的 问题 变 成 了 如 何 根 据 图 像 中 某 个 特 
征 点 得 找 字 典 中 相应 的 单词 。 

仍然 有 朴素 的 思想 : 只 要 和 每 个 单词 进行 比 对 ， 取 最 相似 的 那个 就 
可 以 了 一 一 这 当然 是 简单 有 效 的 做 法 。 然 而 ， 考 虑 到 字 — 典 的 通用 性 中 ， 
我 们 通常 会 使 用 一 个 较 大 规模 的 字典 ， 以 保证 当前 使 用 环境 中 的 图 像 特 
征 都 曾 在 字典 里 出 现 ， 或 至 少 有 相近 的 表达 。 如 果 你 觉得 对 十 个 单词 一 
一 比较 不 是 什么 及 烦 事 ， 那 么 对 于 一 万 个 昵 ? 十 万 个 呢 ? 

也 许 读者 学 过 数据 结构 ， 这 种 O_ (n ) 的 查找 算法 显然 不 是 我 们 想 要 
的 。 如 条 字 和 典 排 过 序 ， 那 么 二 分 得 找 显 然 可 以 提升 得 找 效率 ， 达 到 对 数 
级 别 的 复杂 上 度 。 而 实践 当中 ， 我 们 可 能 会 用 更 复杂 的 数据 绪 构 ， 例 如 
FabmapP^9596! 中 的 Chou-Liu tree?! 等。 但 我 们 不 想 把 本 书写 成 复 森 细 市 
的 集合 ， 所 以 介绍 男 一 种 较为 简单 实用 的 树 结构 381 。 

在 文献 [98] 中 ， 使 用 一 种 K X Bote. "AGRIS CU 
图 12-4 所 示 ) ， 类 似 于 层次 聚 类 ， 是 k-means 的 直接 扩展 。 假 定 我 们 有 
个 特征 点 ， 和 希望 构建 一 个 深度 为 d 、 每 次 分 又 为 K 的 树 ， 那 么 做 法 如 下 叶 


1. 在 根 和 节点， 用 k-means 把 所 有 样本 聚 成 K 类 【实际 中 为 保证 聚 类 均 
匀 性 会 使 用 k-means++) 。 这 样 得 到 了 第 一 层 。 

2. 对 第 一 层 的 每 个 节点 ， 把 属于 该 市 点 的 样本 再 聚 成 k 类 ， 得 到 下 
= 

3. 以 此 类推 ， 最 后 得 到 叶子 层 。 时 子 层 即 为 所 谓 的 Words。 





但 找 茶 特征 时 的 路 径 
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图 12-4 K 叉 树 字 典 示 童 图。 训练 字典 时 ， 逐 层 使 用 K-means 聚 类 。 根 据 已 知 特征 但 找 早 
词 时 ， 亦 可 逐 层 比 对 ， 找 到 对 应 的 里 词 。 


实际 上 ， 最 终 我 们 仍 在 叶子 层 构 建 了 和 蛙 词 ， 而 树 结 构 中 的 中 间 节 反 
仅 供 快速 得 找 时 使 用 。 这 样 一 个 K Apoc. TREE Ad 的 树 ， 可 以 容纳 友 个 
单词 。 羽 一 方面 ， 在 碍 找 示 个 给 定 特征 对 应 的 单词 时 ， 只 需 将 它 与 每 个 
中 间 节 点 的 聚 类 中 心 比较 (一 共 q 7k) ， 即 可 找到 最 后 的 单词 ， 保 证 了 
Xf ATI HY ERICE 


12.3.2 See: 创建 字典 


既然 讲 到 了 字典 生成 ， 我 们 就 来 实际 演示 一 下 。 前 面 的 VO 部 分 大 
量 使 用 了 ORB 特 征 描述 ， 所 以 这 里 就 来 演示 一 下 如 何 生成 及 使 用 ORB 字 
典 。 


ZN 


本 实验 中 ， 我 们 选取 TUM 数 据 集 中 的 10 幅 图 像 〈 位 于 
slamboolkvch9/data 中 ， 如 图 12-5 所 示 ) ， 它 们 来 目 一 组 实际 的 相机 运动 
轨迹 。 可 以 看 出 ， 第 一 幅 图 像 与 最 后 一 幅 图 像 明显 采 目 同一 个 地 方 ， 现 
在 我 们 要 看 程序 能 个 检测 到 这 件 事 情 。 根 据 词 袋 模型 ， 我 们 先 来 生成 这 


十 张 图 像 对 应 的 字典 。 
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图 12-5 ”演示 实验 中 使 用 的 十 幅 图 像 ， 采 集 目 不 同时 刻 的 轨迹 。 


项 要 声明 的 是 ， 实 际 BoW 使 用 时 字典 往往 是 从 更 大 的 数据 集 生 成 
的 ， 而 且 最 好 是 来 目 与 目标 环境 类 似 的 地 方 。 我 们 通 单 使 用 较 大 规模 的 
衬 典 一 一 越 六 代表 字典 单词 量 越 丰 昌 ， 越 容 多 找到 与 当前 图 像 对 应 的 单 
词 ， 但 也 不 能 大 到 超过 我 们 的 计算 能 力 和 内 存 。 笔 者 个 打算 在 GitHub 上 
存放 一 个 很 大 的 字典 文件 ， 所 以 我 们 暂时 从 十 幅 图 像 训 练 一 个 小 的 子 
典 。 如 条 读者 四 进一步 退 求 更 好 的 效 朱 ， 应 该 下 载 更 多 的 数据 ， 训 练 更 
大 的 字典 ， 这 样 程序 才 会 实用 。 也 可 以 使 用 别人 训练 好 的 字典 ， 但 请 注 
意 字 和 典 使 用 的 特征 区 型 是 合 一 致 。 


下 面 开始 训练 人 字典。 前 和 完 ， 请 安 北 本 程 厅 使 用 的 BoW 库 。 我 们 使 用 
7 DBoW3"! : https://github.com/rmsalinas/DBow3。 读 者 也 可 从 本 书 代 
但 的 3rdparty 文 件 夹 中 找到 它 ， 它 是 一 个 cmake 工 程 。 


接 下 来 考虑 训练 字典 : 


中 slambook/ch9/feature training.cpp 





int main( int argc, char** argv ) 
1 
// read the image 


cout<<"reading images... "««endl; 
Vector<Mat> images; 
for ( int i20; i<10; i++ ) 


1 


N ON un + w N — 


8 string path = "./data/"+to_string(i+1)+".png"; 


9 images.push_back( imread(path) ); 

10 } 

1] 

12 // detect ORB features 

13 cout<<"detecting ORB features ... "««endl; 
14 Ptr« Feature2D > detector = ORB::create(); 
15 Vector<Mat> descriptors; 

16 for ( Mat& image:images ) 

17 { 

18 vector<KeyPoint> keypoints; 

19 Mat descriptor; 

20 detector->detectAndCompute( image, Mat(), keypoints, descriptor ); 
21 descriptors.push_back( descriptor ); 
22 } 

23 

24 // create vocabulary 

25 cout<<"creating vocabulary ... "««endl; 

26 DBoW3::Vocabulary vocab; 

27 vocab.create( descriptors ); 

28 cout<<"vocabulary info: "««vocab««endl; 

29 vocab.save( "vocabulary.yml.gz" ); 

30 cout<<"done"<<end1; 

31 

32 return 0; 

3 |} 


DBoW3 的 使 用 非常 容易 。 我 们 对 十 张 目 标 图 像 提 取 ORB 特 征 并 存 
放 至 vector 容 需 中 ， 然 后 调用 DBoW3 的 字典 生成 接口 即 可 。 在 
DBowW3::Vocabulary 对 象 的 构造 函数 中 ， 我 们 能 够 指定 树 的 分 又 数量 及 
深度 ， 不 过 这 里 使 用 了 上 默认 构造 函数 ， 也 束 是 k -10,d =5。 这 是 一 个 小 
规模 的 字典 ， 最 大 能 容纳 10,000 个 单词 。 对 于 图 像 特征 ， 我 们 亦 使 用 默 
认 参 数 ， 即 每 幅 图 像 500 个 特征 点 。 最 后 ， 我 们 把 字 暴 存储 为 一 个 压缩 
X. 


运行 此 程序 ， 将 看 到 如 下 字典 信息 输出 ; 


1 |$ build/feature training 

2 |reading images... 

3 | detecting ORB features ... 

4 | creating vocabulary ... 

s | Vocabulary info: Vocabulary: k = 10, L = 5, Weighting = tf-idf, Scoring = Li-norn, 
Number of words - 4983 


6 | done 





我 们 看 到 : 分 支 数量 k 7310. REL AS! ， 单 词 数 量 为 4983， 没 有 


达到 最 大 容量 。 但 是 ， 剩 下 的 Weighting 和 Scoring 是 什么 呢 ? 从 字面 上 


看 ， 
WE ? 


Weighting 古 权重 ，Scoring 似 乎 指 的 是 评分 ， 但 评分 和 是 如 何 计算 的 


12.4 相似 度 计算 
12.4.1 理论 部 分 


下 面 我 们 来 讨论 相似 上 度 计算 的 问题 。 有 了 字典 之 后 ， 给 定 任 意 特 征 
厂 ， 只 要 在 字典 树 中 逐 层 查找 ， 节 后 都 能 找到 与 乙 对 应 的 单词 w 一 一 当 
字典 足够 大 时 ， 我 们 可 以 认为 上 Mw, 来 目 同一 类 物体 《尽管 没 有 理论 上 
的 保证 ， 仅 是 在 聚 类 意义 下 这 样 说 ) 。 那 么 ， 假 设 一 幅 图 像 中 提取 了 
个 特征 ， 找 到 这 六 个 特征 对 应 的 单词 之 后 ， 我 们 束 相 当 于 拥有 了 访 图 像 
在 单词 列表 中 的 分 布 ， 或 者 直方 图 。 直 观 地 讲 (或 理想 情况 下 ) ， 相 当 
于 是 说 “这 幅 图 里 有 一 个 人 和 一 辆 汽车 ”这 样 的 意思 了 。 根 据 Bag-of- 
Words 的 说 法 ， 不 芒 认 为 这 是 一 个 Bag。 


注意 到 这 种 做 法 中 我 们 对 所 有 单词 都 是 "一视同仁 ? 的 一 — i x 
有 ， 没 有 吏 是 没有 。 这 样 做 好 不 好 呢 ? 考虑 到 不 同 的 单词 在 区 分 性 上 的 
重要 性 并 不 相同 。 例 如 , “的 “是 ”这 样 的 字 可 能 在 许 许多 多 的 句子 中 出 
现 ， 我 们 无 法 根据 它们 判别 句子 的 类 型 ， 但 如 果 有 “文档 “ 足 球 ” 这 样 的 
单词 ， 对 判别 句子 的 作用 就 大 一 些 ， 可 以 说 它们 提供 了 更 多 信息 。 所 以 
概括 起 来 ， 我 们 希望 对 单词 的 区 分 性 或 重要 性 加 以 评估 ， 给 它们 不 同 的 
权 值 以 起 到 更 好 的 效果 。 

在 文本 检索 中 ， 第 用 的 一 种 做 法 称 为 TF-IDF (Term Frequency- 
Inverse Document Frequency) H0104 , meds v y PU. TERI 
HERE. ae Fie] iW AREAS HE. EWK. FAT 
fl, IDFA AE, EE A SL SR, WP 2S RY DX 
4] BEER TAY o 

在 词 袋 模型 中 ， 在 建立 字典 时 可 以 考虑 IDE 部 分 。 我 们 统计 茶 个 叶 
子 方 尽 w， 中 的 特征 数量 相对 于 所 有 特征 数量 的 比例 作为 IDF 部 分 。 假 设 
所 有 特征 数量 为 n w 数量 为 nm ， 那 么 该 单词 的 IDE 为 


IDF; = log —. (12.5) 
Tli 


另 一 方面 ，TEF 部 分 则 是 指 茶 个 特征 在 单 幅 图 像 中 出 现 的 频率 。 假 设 
图 像 A 中 单词 w HI Fn 次 ， 而 一 共 出 现 的 单词 次 数 为 n ， 那 么 TF 为 


TE; = —. (12.6) 


FÆ, w 的 权重 等 于 TF 乘 IDF 之 积 : 


考虑 权 昔 以后， 对 于 某 幅 图 像 A  ， 它 的 特征 点 可 对 应 到 许多 个 单 
词 ， 组 成 它 的 Bagof-Words: 


A= {(w1,71), (wa, N2) yttt3 (WwN, nN)} 会 vw. (12.8) 


由 于 相似 的 特征 可 能 沙 到 同一 个 关中， 因此 实际 的 w 中 会 存在 大 量 
的 雪 。 无 论 如 何 ， 通 过 词 袋 我 们 用 单个 同 量 w fO — SERA. XUT 
器 量 v。 古 一 个 稀 牙 的 同 量 ， 它 的 非 零 部 分 指示 了 图 像 A 中 含有 哪些 单 
词 ， 而 这 些 部 分 的 值 为 TF-IDF 的 值 。 

接 下 来 的 问题 是 : 给 定 w 和 ww ， 如 何 计算 它们 的 差异 呢 ?” 这 个 问题 
和 汇 数 定义 的 方式 一 样 ， 存 在 夺 干 种 解决 方式 ， 比 如 文献 [102] 中 提 人 到 的 
L , WAS: 


N 
s(vA— vp) = 2》 [vail + |vpi| — |vAi — vBi|. (12.9) 
i] 


当然 也 有 很 多 种 别 的 方式 等 你 探索 ， 在 这 里 我 们 仅 举 一 例 作 为 泪 
未 。 全 上 此， 我 们 已 说 明了 如 何 通 过 词 袋 模型 来 计算 任意 图 像 间 的 相似 
及 。 下 面 通过 程序 实际 演练 一 下 。 


12.4.2 ER: 相似 度 的 计算 


[4]. 
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四 slambook/ch9/loop closure.cpp 





int main( int argc, char** argv ) 


{ 


// read the images and database 

cout<<"reading database"««endl; 

DBoW3:: Vocabulary vocab("./vocabulary.yml.gz"); 
if ( vocab.empty() ) 


1 
cerr<<"Vocabulary does not exist."««endl; 
return 1; 

} 

cout<<"reading images... "««endl; 


Vector<Mat> images; 
for ( int i20; i410; i++ ) 
1 
string path = "./data/"*to string(i*i)*".png"; 


images.push back( imread(path) ) ; 


// NOTE: in this case we are comparing images with a vocabulary generated by 
themselves, this may leed to overfitting. 

// detect ORB features 

cout<<"detecting ORB features ... "««endl; 

Ptr« Feature2D > detector = ORB::create(); 

Vector<Mat> descriptors; 


for ( Mat& image:images ) 


1 
Vector<KeyPoint> keypoints; 
Mat descriptor; 
detector->detectAndCompute( image, Mat(), keypoints, descriptor ); 
descriptors.push back( descriptor ); 
} 


// we can compare the images directly or we can compare one image to a database 
// images 
cout<<"comparing images with images "««endl; 


for ( int i=0; i<images.size(); i++ ) 


{ 
DBoW3::BowVector v1; 
vocab.transform( descriptors[i], v1 ); 
for ( int j=i; j<images.size(); j++ ) 
1 
DBoW3::BowVector v2; 
vocab.transform( descriptors[jl, v2 ); 
double score = vocab.score(vi, v2); 
cout««"image "««i««" vs image "««j««" : "««score««endl1; 
) 
cout««endl; 
I 


// or with database 
cout<<"comparing images with database "««endl; 
DBoW3::Database db( vocab, false, 0); 
for ( int i=0; i«descriptors.size(); i++ ) 
db.add(descriptors[i]); 
cout<<"database info: "««db««endl; 
for ( int i=0; i«descriptors.size(); i++ ) 
1 
DBoW3::QueryResults ret; 
db.query( descriptors[i], ret, 4); // maz result=4 


cout<<"searching for image "<<i<<" returns "««ret««endl««endl; 


61 cout««"done."««endl; 





本 程序 演示 了 两 种 比 对 方式 : 图 像 之 间 的 下 接 比 较 ， 以 及 图 像 与 数 
据 库 之 间 的 比较 一 一 尽管 它们 是 大 同 小 异 的 。 此 外 ， 我 们 输出 了 每 幅 图 
像 对 应 的 Bag-of-Words 摘 述 同 量 ， 恋 者 可 以 从 输出 数据 中 看 到 和 它们。 


1 |$ build/feature training 

2 |reading database 

3 |reading images... 

4 |detecting ORB features ... 

5 |comparing images with images 

6 |desp 0 size: 500 

7 | transform image 0 into BoW vector: size = 455 

8 |key value pair = «1, 0.00155622>, «3, 0.00222645>, «12, 0.00222645>, «13, 
0.00222645>, «14, 0.00222645>, «22, 0.00222645>, «33, 0.00222645>, <37, 
0.00155622>, «38, 0.00222645>, «39, 0.00222645>, «43, 0.00222645>, «57, 0.00155622» 





可 以 看 到 ，Bow 摘 述 同 量 中 全 有 每 个 单词 的 ID 和 权重 ， 它 们 构成 了 
整个 稀疏 的 问 量 。 当 我 们 比较 两 个 同 量 时 ，DBowW3 会 为 我 们 计算 一 个 
分 数 ， 计 算 的 方式 由 之 前 构造 字典 时 定义 : 


2 : 0.0234552 
3 : 0.0225237 
4 : 0.0254611 
5 : 0.0253451 
6 : 0.,0272251 
7 : 0.0217745 
8 : 0.0231948 
9 : 0.0311284 


10 : 0.0525447 





在 数据 库 碍 询 时 ，DBowW 对 上 面 的 分 数 进行 排序 ， 给 出 最 相似 的 结 
R: 





1 | searching for image 0 returns 4 results: 
2 |<EntrylId: 0, Score: 1» 

3 |<EntryId: 9, Score: 0.0525447> 

4 |<EntryId: 8, Score: 0.0311284> 

s |<Entryld: 5, Score: 0.0272257» 


7 | searching for image 1 returns 4 results: 
s |<Entryld: 1, Score: 1> 
9 |<EntryId: 2, Score: 0.0339641> 


«EntryId: 
«EntryId: 


searching 
«EntryId: 
«EntryId: 
«EntryId: 
«EntryId: 


searching 
<EntrylId: 
<EntrylId: 
«EntryId: 
«EntryId: 


searching 
«EntryId: 
«EntryId: 
<EntrylId: 
<EntrylId: 


searching 
«EntryId: 
«EntryId: 
«EntryId: 
«EntryId: 


searching 
«EntryId: 
«EntryId: 
«EntryId: 
«EntryId: 


searching 
«EntryId: 
«EntryId: 
<Entryld: 
«EntryId: 


searching 
«EntryId: 
«EntryId: 
«EntryId: 
«EntryId: 


8, Score: 


3, Score: 


for image 
2, Score: 
7, Score: 
9, Score: 


1, Score: 


for image 
3, Score: 
9, Score: 
8, Score: 


5, Score: 


for image 
4, Score: 
5, Score: 
0, Score: 


6, Score: 


for image 
5, Score: 
4, Score: 
9, Score: 


6, Score: 


for image 
6, Score: 
8, Score: 
5, Score: 


3, Score: 


for image 
T, Score: 
2, Score: 
1, Score: 


0, Score: 


for image 
8, Score: 
9, Score: 
0, Score: 


6, Score: 


0.0299387» 
0.0256668» 


2 returns 4 


1» 
0.036092» 
0.0348702» 
0.0339641» 


3 returns 4 


1» 

0.0357317» 
0.0278496» 
0.0270168» 


4 returns 4 


1» 

0.0493492» 
0.0253451» 
0.0253017» 


5 returns 4 


1» 
0.0493492» 
0.028996» 
0.0277584» 


6 returns 4 


1» 

0.0306241» 
0.0277584» 
0.0267135» 


7 returns 4 


1» 
0.036092» 
0.0239091» 
0.0231948» 


8 returns 4 


1» 

0.0329149» 
0.0311284» 
0.0306241> 


results: 


results: 


results: 


results: 


results: 


results: 


results: 


55 | searching for image 9 returns 4 results: 
56 | XEntryId: 9, Score: 1> 

57 | XEntryId: 0, Score: 0.0525447> 

ss | XEntryId: 3, Score: 0.0357317» 

59 | XEntryId: 2, Score: 0.0348702> 





读者 可 以 但 看 所 有 的 输出 ， 看 看 个 同 图 像 与 相似 图 像 评 分 有 多 大 到 
异 。 我 们 看 到 ， 明 显 相 似 的 图 1 和 图 10 (在 C++ 中 下 标 为 分 别 为 0 和 
9) ， 其 相似 度 评 分 约 0.0525; 而 其 他 图 像 约 为 0.02。 


在 本 市 的 演示 实验 中 ， 我 们 看 到 相似 图 像 1 和 10 的 评分 明显 融 于 其 
他 图 像 对 ， 然 而 就 数值 上 看 并 没有 我 们 想象 的 那么 明显 。 按 说 如 果 目 己 
和 目 己 比较 相似 上 度 为 100%， 那 么 我 们 《〈 从 人 类 角度 ) 认为 图 1 和 图 10 至 
少 也 有 百 分 之 七 八 十 的 相似 上 度 ， 而 其 他 图 可 能 为 百 分 之 二 三 十 。 然 而 实 
验 结果 却 是 无 关 图 像 约 2%， 相 似 图 像 约 5%， 似 乎 没有 我 们 想象 的 那么 


明显 。 这 和 古人 否 征 我 们 想 要 看 到 的 结束 呢 ? 


12.5.1 增加 字典 规模 


在 机 颖 学 习 领 域 ， 如 果 代 码 没 有 出 错 而 结 采 不 满意 ， 我 们 前 先 怀 
疑 “ 网 络 结构 是 否 够 大 ， 层 数 是 否 足 够 深 ， 数 据 样本 是 否 够 多 ”等 ， 这 依 
然 是 出 于 “好 模型 政 不 过 位 数据 ”的 大 原则 (一 方面 也 是 因为 缺乏 更 深层 
次 的 理论 分 析 ) 。 尺 管 我 们 现在 是 在 研究 SLAM， 但 出 现 这 种 情况 ， 我 
们 前 先 会 怀疑 : 是 不 是 字典 选 得 太 小 了 ? 毕竟 我 们 是 从 十 幅 图 生成 的 字 
典 ， 然 后 义 根 据 这 个 字典 来 计算 图 像 相似 性 。 

slambook/ch9/vocab_larger.yml.gz 古 我 们 生成 的 一 个 稍微 大 一 点 儿 的 
字典 一 一 事实 上 是 对 同一 个 数据 序列 的 所 有 图 像 生 成 的 ， 大 约 有 2,900 
幅 图 像 。 字 上 典 的 规模 仍然 取 k =10,d =5， 即 最 多 一 万 个 单词 。 读 者 可 以 
使 用 同 目 录 下 的 gen_vocab_large.cpp 文 件 日 行 训 练 字 典 。 请 注意 ， 夯 要 
训练 大 型 字典 ， 可 能 需要 一 台 内 存 较 大 的 机 项 ， 并 且 了 耐心 等 上 一 段 时 
间 。 我 们 对 上 一 和 的 程序 稍 加 修改 ， 使 用 更 大 的 字典 去 检测 岁 像 相似 
性 : 





1 |comparing images with database 
2 |database info: Database: Entries = 10, Using direct index = no. Vocabulary: k = 10, 
L = 5, Weighting = tf-idf, Scoring = Li-norm, Number of words = 99566 
searching for image 0 returns 4 results: 
<EntryId: 0, Score: 1> 
<EntryId: 9, Score: 0.0320906» 
<EntryId: 8, Score: 0.0103268> 
4 


<EntryId: 4, Score: 0.0066729> 


Mo oo N [22] un = Ww 


searching for image 1 returns 4 results: 


«EntryId: 
«EntryId: 
<Entryld: 
<Entryld: 


searching 
<Entryld: 
«EntryId: 
<Entryld: 
<Entryld: 


searching 
«EntryId: 
«EntryId: 
«EntryId: 
«EntryId: 


searching 
<Entryld: 
«EntryId: 
<Entryld: 
«EntryId: 


searching 
«EntryId: 
«EntryId: 
<Entryld: 
<Entryld: 


searching 
«EntryId: 
«EntryId: 
<Entryld: 
«EntryId: 


searching 
«EntryId: 
<Entryld: 
«EntryId: 
«EntryId: 


searching 
«EntryId: 
<Entryld: 
«EntryId: 


for image 
2, Score: 
1, Score: 
5, Score: 
8 


, Score: 


for image 
3, 


, Score: 


Score: 


Score: 


, Score: 


for image 
4, 


, Score: 


Score: 


Score: 


, Score: 


for image 
5, Score: 
3, Score: 
2, Score: 
= 


, Score: 


for image 
6, Score: 
T, Score: 
3, 
a 


, Score: 


Score: 


for image 
T, Score: 
6, Score: 
8, Score: 
1, Score: 
for image 
8, Score: 
0, Score: 


2, Score: 


1» 


: 0.0238409> 
: 0.00814409> 
: 0.00697527> 


2 returns 4 
1» 
0.0238409» 
0.00897928» 
0.00893477» 


3 returns 4 
1» 

0.0107005» 
0.00870392» 
0.00720695» 


4 returns 4 
1» 

0.0069998» 
0.0066729» 
0.0062834» 


5 returns 4 
1» 
0.0107005» 
0.00897928» 
0.0062834» 


6 returns 4 
1» 

0.00915307» 
0.00720695» 
0.0069998» 


7 returns 4 
1» 

0.00915307» 
0.00814517» 
0.00538609» 


8 returns 4 
1» 

0.0103268> 
0.00893477> 


results: 


results: 


results: 


results: 


results: 


results: 


results: 


55 | XEntryId: 3, Score: 0.00870392> 


57 | searching for image 9 returns 4 results: 
9, 
0, 
8, 
1, 


ss | <Entryld: 
59 | XEntryIGd: 
60 | XEntryId: 
6&1 | <Entryld: 


Score: 1» 

Score: 0.0320906» 
Score: 0.00636511> 
Score: 0.00587605> 





可 以 看 到 ， 当 和 字典 规模 增加 时 ， 无 天 图 像 的 相似 性 明显 变 小 了 。 而 
相似 的 图 像 ， 例 如 图 像 1 和 10， 虽 然 分 值 也 略 徽 下降， 但 相对 于 其 他 图 
像 的 评分 ， 却 变 得 更 为 显 兰 了 了。 这 说 明 增 加 字 奥 训练 样本 是 有 益 的 。 同 
理 ， 读 者 可 以 等 试 合用 更 大 规模 的 字典 ， 看 看 结 来 会 及 生 怎 样 的 变化 。 


12.5.2 ”相似 性 评分 的 处 理 


对 任意 两 幅 图 像 ， 我 们 都 能 给 出 一 个 相似 性 评分 ， 但 是 只 利用 这 个 
分 值 的 绝对 大 小 并 不 一 定 有 很 好 的 帮助 。 比 如 说 ， 有 些 环境 的 外 观 本 来 
束 很 相似 ， 像 办 公 室 往往 有 很 多 同 蒜 式 的 加 榭 ; 力 一 些 环境 则 各 个 地 方 
部 有 有 很 大 的 个 同 。 郊 虑 到 这 种 情况 ， 我 们 会 取 一 个 先 验 相 似 度 s (V, ,vi A， 
)， 它 表示 汞 时 刻 关 键 帆 图 像 与 上 一 时 刻 的 关键 帧 的 相似 性 。 然 后 ， 其 


他 的 分 值 痢 参照 这 个 值 进行 归 一 化 : 
s (v, vi.) = s (vi, vi.) /5 (Vt, Ut - Ac) - (12.10) 


站 在 这 个 角度 上 上， 我 们 说 : UR Ud HR BE pi BR TH A GR 
过 当前 帧 与 上 一 个 关键 帧 相似 度 的 3 倍 ， 就 认为 可 能 存在 回环 。 这 个 步 
又 避免 了 引入 绝对 的 相似 性 国 值 ， 使 得 算法 能 够 适应 更 多 的 环境 。 


12.5.3 E BE DOLES] AE E 


TES M [e] PRESE, RAUMA RUE] EEK. ELA EME FAN 
Xm. ABA SBT KEMBLA IPE ES. FEZ PAR ol h 


FA Sara PATA. EGO, MUZE RA A ce Bin MTA SR n- Ql. n- 3 帧 
最 为 相似 ， 这 种 结 末 似乎 太平 凡 了 ， 意 义 不 大 。 所 以 从 实践 上 说 ， 用 于 
le FA ae EA Mo eae dee ee Pn EE, BZ ASAHI], RER ae BETS 
M 


FAA FT Bl, WAR RT ee BY TBA, Pea BUE TS IIDUNI n 
bi. ABARAT REZ +1. Bn -2 UE A AS LO IAI. (Axe, WAU 
MAM san WZ LE EAT LE he Ae BO, Fe PZBUSn 
IW En v2 aS SS ATOR LR Jo 8| A A 7E BR CHK AK, BVA 
AA 16525 HZ Wi is EVR BR Y RRRA BEE AIBA SE AN ae RE H 
HE. MA, RIRES BS IBI AER DA. EGE AN 2 al 
同一 类 的 回环 。 


12.5.4 检测 之 后 的 验证 


词 袋 的 回环 检测 算法 完全 依 顿 于 外 观 而 没有 利用 任何 的 几何 信息 ， 
这 导致 外 观 相似 的 图 像 容 易 被 当成 回环 。 并 且 ， 由 于 词 袋 不 在 乎 单词 顺 
Fe, ATER HR) A CIN ZA A, A a Sl RAE ATLA, EEA 
AM Za RINE RS SE Do 。 


验证 的 方法 有 很 多 。 其 一 是 设立 回环 的 缓存 机 制 ， 认 为 单 识 检 测 到 
的 回环 并 不 足以 构成 民 好 的 约束 ， 而 在 一 段 时 间 中 一 直 检测 到 的 回环 ， 
才 认 为 是 正确 的 回环 。 这 可 以 看 成 时 间 上 的 一 致 性 检测 。 态 一 方法 是 罕 
闻 上 上 的 一 致 性 检 刷 ， 即 对 回环 检测 到 的 两 个 病 进 行 特征 匹配 ， 佑 计 相 机 
的 运动 。 然 后 ， 骨 把 运动 放 到 之 前 的 Pose Graph 中 ， 检 奉 与 之 前 的 估计 
是 耕 有 很 大 的 出 入 。 忆 之， 验证 部 分 通 第 是 必需 的 ， 但 如 何 实现 却 是 见 
仁 见 吞 的 问题 。 


12.5.5 与 机 器 学 习 的 关系 


从 前 面 的 论述 中 可 以 看 出 ， 回 环 检 测 与 机 右 学 习 有 看 干 丝 万 缕 的 关 
联 。 回 环 检 测 本 里 非 第 像 是 一 个 分 类 问题 。 与 传统 模式 识 列 的 区 列 在 
于 ， 回 环 中 的 类 列 数量 很 大 ， 而 每 类 的 样本 很 少 一 一 极 闹 情况 下 ， 妆 机 
WAR EEJ, BARER, WE TIIRA, RAEE NJE 





类 别 当 成 连续 变量 而 非 离散 变量 ; 而 回环 检测 ， 相 当 于 两 幅 图 像 疾 入 同 
一 类 ， 则 是 很 少 出 现 的 。 从 另 一 个 角度 看 ， 回 环 检 训 也 相当 于 对 “图 候 
半 相 似 性 ?概念 的 一 个 学 习 。 既 然 人 次 能 够 掌握 图 像 是 否 相似 的 判断 ， 
让 机 需 学 习 到 这 样 的 概念 也 是 非 第 有 可 能 的 。 

从 词 袋 模型 来 说 ， 筷 本 刁 是 一 个 非 监督 的 机 器 学 习 过 程 一 一 构建 词 
典 相 当 于 对 特征 描述 子 进行 聚 类 ， 而 树 只 是 对 所 聚 的 类 的 一 个 快速 查找 
的 数据 结构 而 已 。 既 然 是 聚 类 ， 结 合 机 右 学 习 里 的 知识 ， 我 们 人 至 少 可 以 
问 : 


1. 是 否 能 对 机 器 学 习 的 图 像 特征 进行 聚 类 ， 而 不 是 对 SURF、ORB 
这 样 的 人 工 设 计 特 征 进行 聚 类 ? 

2. 是 人 耕 有 更 好 的 方式 进行 聚 类 ， 而 不 是 用 树 结 构 加 上 K-means 这 些 
较 朴 素 的 方式 ? 


结合 目前 机 器 学 习 的 发 展 ， 二 进 制 描述 子 的 学 习 和 无 监督 的 聚 类 ， 
都 是 非常 有 人 望 在 深度 学 习 框 染 中 得 以 解决 的 问题 。 我 们 也 陆续 看 到 了 利 
用 机 红学 习 进 行 回环 检测 的 工作 。 尽 管 目前 词 袋 方法 仍 是 主流 ， 但 笔者 
本 人 相信 ， 未 来 深度 学 习 方 法 很 有 和 硕 望 打败 这 些 人 工 设计 特征 的 、“ 传 
统 ” 的 机 项 学 习 方 法 4105 。 毕 葛 词 袋 方法 在 物体 识 询 问题 上 已 经 明显 不 
如 神经 网 络 了， 而 回环 检测 又 是 非常 相似 的 一 个 问题 。 

习题 

1. 请 书写 计算 PR 曲线 的 小 程序 。 用 MATLAB 或 Python 可 能 更 加 简便 
一 些 ， 因 为 它们 擅长 作 图 。 


2. 验 证 回环 检 训 算法 ， 需 要 有 人 工 标记 回环 的 数据 集 ， 例 如 [94]。 
然而 人 工 标记 回环 是 很 不 方便 的 ， 我 们 会 考虑 根据 标准 轨迹 计算 回环 。 
BY, WAR LE FA PASE SE A, DUA Ee. TARE 
TUM 数 据 集 给 出 的 标准 轨迹 ， 计 算出 一 个 数据 集中 的 回环 。 这 些 回 环 
的 图 像 真 的 相似 吗 ? 


3. 学 习 DBoW3 或 DBoW2 库 ， 自 己 寻 找 几 张 图 片 ， 看 能 否 从 中 正确 
检测 出 回环 。 


4. 调 研 相似 性 评分 的 第 用 上 度量 方式 ， 哪 些 比 较 津 用 ? 
5.Chow-Liu 树 和 是 什么 原理 ? 它 是 如 何 被 用 于 构建 字典 和 回环 检测 





HJ? 
6. 疝 读 文 献 [106]， 除 了 词 袋 模型 ， 还 有 哪些 用 于 回环 检测 的 方法 ? 








[1] 有 机 融 学 习 背 景 的 读者 ， 应 该 能 感受 出 这 段 话 与 机 禹 学 习 是 何等 相似 。 你 是 不 是 已 经 在 想 如 
何 训练 网 络 了 呢 ? 


[2] 虽然 这 种 性 质 有 时 也 会 市 来 一 些 问 题 ， 例 如 ， 眼 睛 长 在 嘴巴 下 的 脸 仍 是 人 上 脸 吗 ? 
ee 





[4] 我 们 用 了 K 和 q 表达 树 的 分 文 和 深度 ， 这 可 能 会 令 你 想到 k-d 树 。 笔 者 觉得 虽然 做 法 不 尽 相 
E, BETREE KE M. 


[5] AH AY FE A B] EH OpenC V3 RARER, H mE EHRE EF 
[6] 这 里 的 L 即 前 文 说 的 d 。 
[7] 个 人 觉得 TF-IDF 称 呼 起 来 更 顺口 ， 所 以 后 文 束 用 英文 缩写 而 非 中 文 译文 了 。 





B13 建 图 


主要 目标 

1.38 fe e H SLAMFE J| SR BE f v AJ JS E 

2. 通 过 实验 了 解 单 目 稠 密 重 建 的 过 程 。 

3. 了 解 几 种 RGB-D 重 建 中 的 地 图 形式 。 

本 讲 我 们 开始 介绍 建 图 部 分 的 算法 。 在 前 端 和 后 端 中 ， 我 们 重点 关 
注 同 时 估计 相机 运动 轨迹 与 特征 点 空间 位 置 的 问题 。 然 和 而， 在 实际 使 用 
SLAM 时 ， 除 了 对 相机 本 体 进 行 定 位 之 外 ， 还 存在 许多 其 他 的 需求 。 例 
如 ， 考 虚 放 在 机 器 人 上 的 SLAM， 那 么 我 们 会 希望 地 图 能 够 用 于 定位 、 
导航 、 避 障 和 交互 ， 特 征 点 地 图 显然 不 能 满足 所 有 的 这 些 需 求 。 所 以 ， 
本 讲 我 们 将 更 详细 地 讨论 各 种 形式 的 地 图 ， 并 指出 目前 视 党 SLAM 地 图 
中 存在 痢 的 缺陷 。 


13.1 概述 





建 图 (Mapping) ， 本 应 该 是 SLAM 的 两 大 目标 之 一 一 一 因为 SLAM 
锐 称 为 同时 定位 与 建 图 。 但 是 直到 现在 ， 我 们 讨论 的 都 是 定位 问题 ， 包 
括 骨 过 特征 点 的 定位 、 直 接 法 的 定位 ， 以 及 后 病 优 化 。 那 么 ， 这 是 人 否 暗 
示 建 图 在 SLAM 里 没有 那么 重要 ， 所 以 我 们 直到 本 讲 才 开始 讨论 呢 ? 

答案 是 侍 定 的 。 事 实 上 ， 在 经 典 的 SLAM 模 型 中 ， 我 们 所 谓 的 地 
图 ， 即 所 有 路 标点 的 集合 。 一 旦 确定 了 路 标点 的 位 置 ， 那 束 可 以 说 我 们 
TR SEA. Tae, AU oe BRET th, Bundle Adjustment 也 
好 ， 事 实 上 都 建 模 了 路 标点 的 位 置 ， 并 对 它们 进行 优化 。 从 这 个 角度 上 
说 ， 我 们 已 经 探讨 了 建 图 问题 。 那 么 为 何 还 要 和 蛙 独 列 一 讲 建 图 昵 ? 


这 是 因为 人 们 对 建 图 的 需求 不 同 。SLAM 作 为 一 种 底层 技术 ， 往 往 
是 用 来 为 上 层 应 用 提供 信息 的 。 如 果 上 层 是 机 器 人 ， 那 么 应 用 层 的 开发 
者 可 能 名 望 使 用 SLAM 来 做 全 局 的 定位 ， 并 且 让 机 上 右 人 在 地 图 中 导航 
例如 扫地 机 需要 完成 扫地 工作 ， 和 硕 望 计算 一 条 能 够 颖 兰 整 张 地 图 的 
路 径 。 或 者 ， 如 果 上 层 是 一 个 增强 现实 设备 ， 那 么 开发 者 可 能 希望 将 虐 
拟 物 体 亚 加 在 现实 物体 之 中 ， 特 别 地 ， 还 可 能 需要 处 理 虚拟 物体 和 真实 
物体 的 遮挡 关系 。 


我 们 友 现 ， 应 用 层面 对 于 “定位 ”的 需求 是 相似 的 ， 它 们 希望 SLAM 
提供 相机 或 搭载 相机 的 主体 的 空间 位 姿 信 息 。 而 对 于 地 图 ， 则 存在 着 许 
多 不 同 的 需求 。 在 视觉 SLAM 看 来 , “ 建 图 ”是 服务 于 “定位 ”的 ;但 是 在 
应 用 层面 看 来 , “ 建 图 ?明显 还 市 有 许多 其 他 的 需求 。 关 于 地 图 的 用 处 ， 
我 们 大 致 归纳 如 下 : 

LEM 。 定 位 是 地 图 的 一 项 基本 功能 。 在 前 面 的 视 客 里程 计 部 分 ， 
我 们 讨论 了 如 何 利用 局 部 地 图 来 实现 定位 。 在 回环 检测 部 分 ， 我 们 也 看 
到 ， 只 要 有 全 局 的 朱 述 子 信 息 ， 我 们 也 能 通过 回环 检 训 确定 机 夫人 的 位 
置 。 更 进一步 ， 我 们 还 希望 能 够 把 地 图 保存 下 来 ， 让 机 堪 人 在 下 次 开机 
后 依然 能 在 地 图 中 定位 ， 这 样 只 需 对 地 图 进行 一 次 建 模 ， 而 不 是 每 次 局 
动机 项 人 痢 重 新 做 一 次 完整 的 SLAML。 


2. 寻 航 。 导 航 是 指 机 郝 人 能 够 在 地 图 中 进行 路 径 规划 ， 在 任意 两 个 





地 图 点 间 寻 找 路 径 ， 然 后 控制 目 己 运动 到 目标 点 的 过 程 。 该 过 程 中 ， 我 
们 全 少 吉 要 知 违 地 图 中 哪些 地 方 不 可 退 过 ， 而 哪些 地 方 是 可 以 通过 的 
。 这 束 超 出 了 稀 玩 特征 点 地 图 的 能 力 围 ， 我 们 必须 有 为 外 的 地 图 形 
式 。 稍 后 我 们 会 说 ， 这 至少 得 是 一 种 稠密 的 地 图 。 

3. 避 卫 。 避 障 也 是 机 如 人 经 第 碰 到 的 一 个 问题 。 它 与 导航 类 似 ， 但 
更 注重 局 部 的 、 动 态 的 障碍 物 的 处 理 。 同 样 ， 仅 有 特征 点 ， 我 们 无 法 判 
断 东 个 特征 点 是 售 为 障碍 物 ， 所 以 需要 稠密 地 图 。 


4. 重 建 。 有 时 候 ， 我 们 希望 利用 SLAM 获 得 周围 环境 的 重建 效 束 ， 
并 把 它 展示 给 其 他 人 看 。 这 种 地 图 主要 用 于 同人 人 展示， 所 以 我 们 硕 望 它 
看 上 去 比较 舒服 、 寺 观 。 或 者 ， 我 们 也 可 以 把 访 地 图 用 于 通信 ， 使 其 他 
人 能 够 远程 地 观看 我 们 重建 得 到 的 三 维 物体 或 场景 一 例如 三 维 的 视频 
退 话 或 者 网 上 购物 每 。 这 种 地 图 亦 是 秽 密 。” 的， 并且 我 们 还 对 它 的 外 观 
有 一 些 要 求 。 我 们 可 能 不 满足 于 稠密 后 云 曾 建 ， 喝 布 户 能 够 构建 币 纹 理 
的 平面 ， 束 像 电 子 游 戏 中 的 三 维 场景 那样 。 


5.208 。 交 互 主要 指 人 与 地 图 之 间 的 互动 。 例 如 ， 在 增强 现实 中 ， 
我 们 会 在 房间 里 放置 虚拟 的 物体 ， 并 与 这 些 虚 拟 物 体 之 间 有 一 些 互 动 
一 一 比方 说 我 会 扣 击 墙 面 上 放 看 的 虚拟 网 页 浏览 右 来 观看 视频 ， 或 者 问 
mH PMU, PECIA EMK) 物理 健 揪 。 万 一 方面 ， 机 益 人 应 
用 中 也 会 有 与 人 、 与 地 图 之 间 的 交互。 例如 ， 机 堪 人 可 能 会 收 到 命 
令 “ 取 蝎子 上 的 报纸 >， 那 么 ， 除 了 有 环境 地 图 之 外 ， 机 规 人 还 需要 知道 
uA rU, Bp fp E", TA SHULER’ X EL 
名 人 对 地 图 有 更 局 层面 的 认 知 一 一 亦 称 为 语义 地 图 。 


图 13-1 形 象 地 解释 了 上 面 讨论 的 各 种 地 图 类 型 与 用 途 之 则 的 天 系 。 
我 们 之 前 的 讨论 ， 基 本 集中 于 “ 稀 玩 路 标 地 图 ”部 分 ， 还 没有 探讨 笛 哗 地 
图 。 所 谓 秽 密 地 图 是 相对 于 稀 下 地 图 而 言 的 。 稀 玩 地 图 只 建 模 感 兴趣 的 
部 分 ， 也 束 古 前 面 襄 了 很 久 的 特征 点 路标 皮 )。 而 筒 密 地 图 是 指 ， 建 
模 所 有 ” 看 到 过 的 部 分 。 对 于 同一 张 果子 ， 稀 玩 地 图 可 能 只 建 模 了 果子 
的 四 个 角 ， 而 稠密 地 图 则 会 建 模 整个 条 面 。 虽 然 从 定位 角度 看 ， 只 有 四 
个 角 的 地 图 也 可 以 用 于 对 相机 进行 定位 ， 但 由 于 我 们 无 法 从 四 个 角 推断 
这 几 个 点 之 间 的 空间 结构 ， 所 以 无 法 仅 用 四 个 角 来 完成 导航 、 避 障 等 需 
要 稠密 地 图 才能 完成 的 工作 。 











图 13-1 各 种 地 图 的 示意 图 。 三 种 例子 地 图 分 别 来 目 文 献 [73,107,108]。 


从 上 和 面 的 讨论 中 可 以 看 出 ， 稠 密 地 图 占据 着 一 个 非常 竺 要 有 的 位 置 。 
于 是 ， 剩 下 的 问题 是 : 通过 视觉 SLAM 能 建立 稠密 地 图 吗 ? 如 果 能 ， 怎 
么 建 呢 ? 


13.2 RH Ji E E 
13.2.1 立体 视觉 


视觉 SLAM 的 稠密 重建 问题 是 本 讲 的 第 一 个 重要 话题 。 相 机 ， 很 久 
以 来 被 认为 是 只 有 角度 的 传感器 (Bearing only) 。 单 幅 图 像 中 的 像素 ， 
只 能 提供 物体 与 相机 成 像 平 面 的 角度 及 物体 采集 到 的 壳 度 ， 而 无 法 提供 
物体 的 距离 CRange) 。 而 在 稠密 重建 中 ， 我 们 需要 知 关 每 一 个 像 妹 所 
(或 大 部 分 像素 点 ) 的 距离 ， 对 此 大 臻 上 有 如 下 解决 方案 : 


1. 使 用 单 目 相机 ， 通 过 移动 相机 之 后 进行 三 角 化 名 量 像素 的 距离 。 


2. 使 用 双 目 相机 ， 利 用 左右 目的 视 震 计算 像素 的 距离 〈 多 目 原 理 相 
FEJ) 。 

3. 使 用 RGB-D 相 机 直接 获得 像素 距离 。 

前 两 种 方式 称 为 立体 视觉 (Stereo Vision) ， 其 中 移动 单 目 的 又 称 
为 移动 视角 的 立体 视觉 Moving View Stereo) 。 相 比 于 RGB-D 直 接 测 
量 的 深度 ， 蛙 目 和 双 目 对 深度 的 获取 往往 是 “费力 不 讨好 ”的 一 一 我 们 需 
要 花费 大 量 的 计算 ， 最 后 得 到 一 些 不 怎么 可 对 的 后 深度 估计 。 当 然 ， 
RGB-D 也 有 一 些 量程 、 应 用 范围 和 光照 的 限制 ， 不 过 相 比 于 单 目 和 双 目 
的 结果 ， 使 用 RGB-D 进 行 稠 密 重 建 往往 是 更 常见 的 选择 。 而 单 日 、 双 目 
的 好 处 是 ， 在 目前 RGB-D 还 无 法 很 好 应 用 的 室外 、 大 场景 场合 中 ， 仍 能 
通过 立体 视觉 估 计 深 度 信息 。 

话 虽 如 此 ， 本 节 我 们 将 这 领 谈 者 实现 一 明 音 目的 稠密 估计 ， 体 验 为 
何 说 它 是 费力 不 讨好 的 。 我 们 从 最 简单 的 情况 开始 说 起 : 在 给 定 相 机 轨 
迹 的 基础 上 ， 如 何 根 据 一 段 时 间 的 视频 序列 来 估计 某 幅 图 像 的 深度 。 换 
言 之 ， 我 们 不 考虑 SLAM， 先 来 考虑 略为 简单 的 建 图 问题 。 


假定 有 一 段 视频 序列 ， 我 们 通过 霖 种 魔法 得 到 了 每 一 帧 对 应 的 轨迹 
(当然 也 很 可 能 是 由 视觉 里 程 计 前 闹 佑 计 所 得 )。 现 在 我 们 以 第 一 幅 图 
像 为 参考 巾 ， 计 算 参 考 巾 中 每 一 个 像 系 的 深 谋 (或 者 说 距离 )。 肯 先 ， 
请 回忆 在 特征 点 部 分 我 们 是 如 何 完成 该 过 程 的 : 


1. 站 完 ， 我 们 对 图 像 所 取 和 特征， 并 根据 接 述 子 计算 了 特征 之 间 的 区 
Aco PREZ, WORE, RIEA E E AEST SPR, AE SEE 
各 个 图 像 之 则 的 位 置 。 

2. 然 后 ， 由 于 我 们 无 法 仅 用 一 幅 图 像 确 定 特征 氮 的 位 置 ， 所 以 必须 
退 过 不 同 视角 下 的 观测 估计 它 的 深度 ， 原 理 即 前 面 讲 过 的 三 角 测 量 。 


在 稠密 深度 图 佑 计 中 ， 不 同 之 处 在 于 ， 我 们 无 法 把 每 个 像 系 都 当 作 
特征 点 计算 描述 子 。 因 此 ， 笛 密 深 度 估计 问题 中 ， 匹 配 束 成 为 很 重要 的 
一 环 : 如 何 确定 第 一 幅 图 的 条 像 系 出 现在 其 他 图 里 的 位 置 呢 ? 这 需要 用 
到 极 线 搜索 AER DLAC LAR! Ala, SATA SAR MR ATER TEA 
PME. LAE BREE ARE, IRI te EC TARE. AAA 
He, TEXCH BUD] Be eR EAR ET, Me 
Ws BRANT is BERS fr VE BE S Bre ee MA — I ME S A EY Be, 3E 
TMB — SAEI. WEVA UE ae BOAR ATU, PRE AN ARR 
EBSA EUR o 


13.2.2 WWT A SERVE AC 


我 们 先 来 探讨 不 同 视 角 下 观察 同一 个 点 产生 的 几何 关系 。 这 非常 像 
在 第 7.3 节 讨论 的 对 极 几何 关系 。 请 看 图 13-2。 左 边 的 相机 观测 到 了 某 个 
像素 p，。 由 于 这 是 一 个 单 目 相机 ， 我 们 无 从 知道 它 的 深度 ， 所 以 假设 这 
个 深度 可 能 在 某 个 区 域 之 内 ， 不 妨 说 是 某 最 小 值 到 无 穷 远 之 间 : (d,,, 
too )。 因 此 ， 该 像素 对 应 的 空间 点 就 分 布 在 某 条 线段 (本 例 中 是 射线 ) 
上 。 在 另 一 个 视角 “《 右 侧 相 机 ) 看 来 ， 这 条 线段 的 投影 也 形成 图 像 平 面 
上 的 一 条 线 ， 我 们 知道 这 称 为 极 线 ”。 当 知道 两 部 相机 间 的 运动 时 ， 这 
条 极 线 也 是 能 够 确定 的 中 。 那 么 问题 就 是 : 极 线 上 的 哪 一 个 点 是 我 们 刚 
才 看 到 的 p ;点 呢 ? 


可 能 的 涂 度 范围 /, 





图 13-2” 极 线 搜索 示意 图 。 


午 复 一 裔 ， 在 特征 后 方法 中 ， 我 们 通过 特征 匹配 找到 了 p, 的 位 置 。 
然而 现在 我 们 没有 插 述 于 ， 所 以 只 能 在 极 线 上 搜索 和 p , 长 得 比较 相似 的 
扩 。 由 上 其 体 地 说 ， 我 们 可 能 沿 看 第 二 幅 图 像 中 的 极 线 的 攻 一 尖 走 到 为 一 
k, ENERE MAR p, 的 相似 程度 。 从 直接 比较 像 系 的 角度 来 看 ， 
这 种 做 法 倒 生 和 下 接 法 是 异曲同工 的 。 

在 直接 法 的 讨论 中 我 们 知道 ， 比 较 单 个 像素 的 完 度 值 并 不 一 定 稳定 
可 徘 。 一 件 很 明显 的 事情 束 是 : 万 一 极 线 上 有 很 多 和 P , FAA, RI 
怎么 确定 哪 一 个 是 真实 的 呢 ? 这 似乎 回 到 了 我 们 在 回环 检 训 当中 说 到 的 
问题 : 如 何 确定 两 幅 图 像 〈 或 两 个 点 ) 的 相似 性 ? PAE 
来 解决 的 ， 但 这 里 由 于 没有 特征 ， 所 以 只 好 寻求 万 外 的 途径 。 


一 种 下 观 的 想法 是 : BPR MARIN RECA Xe, Abe BAY 


比较 像素 块 呢 ? 我 们 在 p ， 周 围 取 一 个 大 小 为 wxw 的 小 块 ， 然 后 在 极 线 
上 也 取 很 多 同样 大 小 的 小 块 进行 比较 ， 就 可 以 在 一 定 程度 上 提高 区 分 
性 。 这 就 是 所 谓 的 块 岂 配 o TERR PIER PTE, RARER 
像 间 整 个 小 块 的 灰 度 值 不 变 ， 这 种 比较 才 有 意义 。 所 以 算法 的 假设 ， 从 
像素 的 灰 度 不 变性 ， 变 成 了 图 像 块 的 灰 度 不 变性 一 aE EA 
on S o 

好 了 ， 现 在 我 们 取 了 p, 周围 的 小 块 ， 并 且 在 极 线 上 也 取 了 很 多 个 小 
KR. Ap , 周围 的 小 块 记 成 AE Rw， 把 极 线 上 的 n 个 小 块 记 成 B, i 
=1,…,n 。 那 么 ， 如 何 计 算 小 块 与 小 块 间 的 牌 异 呢 ? 有 厂 干 种 不 同 的 计算 
方法 : 

1.SAD (SumofAbsoluteDi ff erence，〉。 顾 名 思 义 ， 即 取 两 个 小 块 的 
到 的 绝对 值 之 和 : 


S(A, B)sap = * |A(i,j) — B(i, j)]. (13.1) 
1,2 
2.SSD。 这 里 的 SSD 并 不 是 指 大 家 玖 芒 的 固态 便 和 三 ， 而 和 是 Sum of 
Squared Distance 〈 平 方 和 ) WAE: 


S(A, B)ss = ? (A(i, j) - B(3))' . (13.2) 


i,j 


3.NCC (Normalized Cross Correlation, H —M 4.48%) 。 这 种 方式 
比 前 两 种 要 复杂 一 些 ， 它 计算 的 是 两 个 小 块 的 相关 性 : 


S Ali, j) Bü, j) 


(13.3) 





S(A, B)ucc = 7 —À) -= 
LAI) 2 Bi, J) 
i,j - 


请 注意 ， 由 于 这 里 用 的 是 相关 性 ， 所 以 相关 性 接近 0 表示 两 幅 图 像 
不 相似 ， 而 接近 1 才 表 示 相 似 。 前 面 两 种 距离 则 是 反 过 来 的 ， 接 近 0 表 示 
相似 ， 而 大 的 数值 表示 个 相似 。 


和 我 们 遇 到 过 的 许多 情形 一 样 ， 这 些 计 算 方 式 往 往 存在 一 个 精度 - 
效率 之 间 的 矛盾 。 精 度 好 的 方法 往往 需要 复杂 的 计算 ， 而 简单 的 快速 算 
法 又 往往 效果 不 佳 。 这 需要 我 们 在 实际 工程 中 进行 取舍 。 另 外 ， 除 了 这 
些 简 单 版 本 之 外 ， 我 们 可 以 先 把 每 个 小 块 的 均值 去 挥 ， 称 为 去 均值 的 
SSD、 去 均值 的 NCC， 等 等 。 去 挥 均值 之 后 ， 我 们 允许 像 “ 小 块 B 比 A 整 
体 上 亮 一 些 ， 但 仍然 很 相似 "这样 的 情况 局 ， 因 此 比 之 前 的 更 加 可 徘 一 
些 。 如 果 读 者 对 更 多 的 块 匹 配 度量 方法 感 兴 趣 ， 建 议 疯 读 文 献 [110,111] 
作为 补充 材料 。 

现在 ， 我 们 在 极 线 上 计算 了 A 与 每 一 个 B, 的 相似 性 上 度量。 为 了 方便 
叙述 ， 假 设 我 们 用 了 NCC， 那 么 ， 我 们 将 得 到 一 个 沿 着 极 线 的 NCC 分 
布 。 这 个 分 布 的 形状 严重 取决 于 图 像 本 里 的 样子 ， 如 图 13-3 所 示 。 在 搜 
逐 距 离 较 长 的 情况 下 ， 我 们 通 闸 会 得 到 一 个 非 凸 函数: 这 个 分 布 存 在 着 
许多 峰值 ， 然 而 真实 的 对 应 点 必定 只 有 一 个 。 在 这 种 情况 下 ， 我 们 会 倾 
器 于 使 用 概率 分 布 来 描述 深度 值 ， 而 非 用 某 个 单一 的 数值 来 描述 深度 。 
于 是 ， 我 们 的 问题 就 转 到 了 在 不 断 对 不 同 图 像 进 行 极 线 搜索 时 ， 我 们 估 
计 的 深度 分 布 将 发 生 怎 样 的 变化 一 一 这 就 是 所 谓 的 深度 滤波 器 。 
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图 13-3 ”匹配 得 分 沿 距离 的 分 布 ， 图 像 来 日 文献 [112]。 
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对 深度 的 分 布 假 设 存 在 着 奋 干 种 不 同 的 做 法 。 首 先 ， 在 比较 简单 的 
假设 条 件 下 ， 我 们 可 以 假设 深度 值 服 从 高 斯 分 布 ， 得 到 一 种 类 卡尔 曼 式 
的 方法 (我 们 稍 后 会 看 到 〉。 而 男 一 方面 ， 在 [112,56] 等 文献 中 ， 亦 采 
用 了 均匀 -高斯 混合 分 布 的 假设 ， 推 导 了 男 一 种 形式 更 为 复杂 的 深度 滤 
波 历 。 本 看 简单 易 用 的 原则 ， 我 们 先 来 介绍 并 尖 示 高 斯 分 布 假设 下 的 深 
度 滤 波 器 ， 然 后 把 均匀 -高 斯 混合 分 布 的 滤波 器 作为 习题 。 

设 某 个 像素 点 的 深度 qd 服从 : 


P(d) = N(p, 07). (13.4) 


MSP HATES, Babs MS CARRE. RE. BRRR 
MLW JS Ee — 7 eg 87 47 i : 


P(dops) = N (Mobs; Obs) (13.5) 


于 是 ， 我 们 的 问题 是 ， 如 何 使 用 观测 的 信息 更 新 原先 d 的 分 布 。 这 
下 是 一 个 信息 融合 问题 。 根 据 附 录 A， 我 们 明白 两 个 高 斯 分 布 的 乘积 依 
然 旦 一 个 高 斯 分 布 。 设 融合 后 的 d 的 分 布 为 Nmese,ohe)， 那么 根据 高 斯 分 
WHIRI, A: 
Hfuse 一 Cob + 7 Hobs Css = 0 Oo, (13.6) 


2 2 fuse 2 Z ? 
o* + C obs o* + O obs 
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轧 融 合 部 分 ， 而 无 须 像 完整 的 卡尔 受 那样 进行 预 负 和 更 新 。 可 以 看 到 融 
合 的 方程 确实 比较 涛 喧 易 情 ， 不 过 问题 仍然 存在 : 如 何 确定 我 们 观测 到 
深度 的 分 布 呢 ? 即 ， 如 何 计算 p 645 ,0 ops VE? 


RFU wo ， 亦 存在 一 些 不 同 的 处 理 方式 。 例 如 ， 文 献 [59] 考 虑 ] 
几何 不 确定 性 和 光度 不 确定 性 三 者 之 和 ， 而 [112] 则 仪 考虑 了 几何 不 确定 
性 。 我 们 暂时 只 考虑 由 几何 关系 带 来 的 不 确定 性 。 现 在 ， 假 设 我 们 通过 
极 线 搜 索 和 块 匹配 确定 了 参考 帧 未 个 像素 在 当前 师 的 投影 位 置 。 那 么 ， 
这 个 位 置 对 深度 的 不 确定 性 有 多 大 呢 ? 

以 图 13-4 为 例 。 考 虑 共识 极 线 搜索 ， 我 们 找到 了 p | 对 应 的 p , ri. M 
MS Pp ,的 深度 仁 ， 认 为 p , 对 应 的 三 维 点 为 P Mit, PWO P 
为 p ，O ,0 ,为 相机 的 平移 t ，O ,P 记 为 a 。 并 且 ， 把 这 个 三 角形 的 下 下 
两 个 角 记 作 a,B 。 现 在 ， 考 虑 极 线 ! ,上 存在 着 一 个 像素 大 小 的 误差 ， 使 
得 B 角 变 成 了 B ， 而 p Hae yp. FI EMA A Ay 。 我 们 要 问 的 
是 ， 这 一 个 像素 的 误差 会 导致 p 与 p 产生 多 大 的 天 距 呢 ? 

WE et 我 们 来 列 写 这 些 量 之 间 的 几何 关系。 显 
a=p-t 
o = arccos (p, t) (13.7) 


B = arccos (a, —t). 





图 13-4 不 确定 性 分 析 。 


对 p ,扰动 一 个 像素 ， 将 使 得 B 产生 一 个 变化 量 68 ， 由 于 相机 焦距 
Af. Ts 


00 = arctan 7 (13.8) 


所 以 : 
B' = 6 + ôf 
y=r—Qa— p. 
于 是 ， 由 正弦 定理 ，p' 的 大 小 可 以 求 得 : 


sin 8’ 
|p" || = |}é|| ——. (13.10) 
sin ^y 


(13.9) 


HHE, RIJE HP EN A EG DEBT IE NE PE UR 
V SR £38 RLM A RAW RE, ALA WICH PUE: 


Tos = ||pll — ||p’|I- (13.11) 
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导 来 放大 这 个 不 确定 性 。 接 下 来 的 深度 数据 融合 ， 已 经 在 前 面 介 绍 过 
了 。 在 实际 工程 中 ， 当 不 确定 性 小 于 一 定 国 值 之 后 ， 残 可 以 认为 深度 数 
Ja C Ze T -o 


ZW LB. BANAT Y VPE AR Pe TE: 

1. paci itque 满足 东 个 初始 的 高 斯 分 布 。 

2. 当 新 数据 产生 时 ， 通 过 极 线 搜 索 和 块 匹配 确定 投影 点 位 置 。 
iiid iue m 


4.34 25 Bip d XE E OX VERS EX UI SE Ta, qus 
回 第 2 步 。 

这 些 步 又 组 成 了 一 玛 可 行 的 深度 估计 方式 。 它 的 实际 结果 如 何 ， 我 
们 将 在 实践 部 分 进行 演示 。 


本 节 的 示例 程序 将 使 用 REMODEH131091 的 测试 数据 集 。 它 提供 了 一 
架 无 人 机 采集 的 单 目 俯视 图 像 ， 共 有 200 张 ， 同 时 提供 了 每 张 图 像 的 真 
实 位 姿 。 下 面 我 们 来 考虑 ， 在 这 些 数据 的 基础 上 估算 第 一 帧 图 像 每 个 像 
素 对 应 的 深度 值 ， 即 进行 单 目 稠密 重建 。 


首先 ， 请 读者 从 http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip 处 下 
载 示 例 程 序 所 用 的 数据 。 你 可 以 使 用 网 页 浏览 如 或 下 载 工 上 共 进 行 下 载 。 
解压 后 ， 将 在 test_data/Images 中 发 现 从 0 至 200 的 所 有 图 像 ， 并 在 
test_data 目 录 下 看 到 一 个 文本 文件 ， 它 记录 了 每 幅 图 像 对 应 的 位 姿 : 






1 | Scene 000.png 1.086410 4.766730 -1.449960 0.789455 0.051299 -0.000779 0.611661 
2 | scene O01.png 1.086390 4.766370 -1.449530 0.789180 0.051881 -0.001131 0.611966 
scene 002.png 1.086120 4.765520 -1.449090 0.788982 0.052159 -0.000735 0.612198 


图 13-5 展 示 了 夯 干 时 刻 的 图 像 。 可 以 看 到 场景 主要 由 地 面 、 果 子 友 
果子 上 的 杂 物 组 成 。 如 果 深 大 估 计 大 至 正确， 那么 我 们 至 少 可 以 看 出 果 
了 与 地 面 的 深度 值 不 同 忆 处。 下 面 ， 我 们 按照 乙 前 的 讲解 书 与 稠密 深度 
估计 程序 。 为 了 方便 理解 ， 程 序 书写 成 了 C 语 言 风 格 ， 放 在 单个 文件 
H. ANREP ARIA ARK, FE EUR LS RB, FORA At Tae 
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Ik) slambook/ch13/dense monocular/dense mapping.cpp (Fr Ex) 





1 | #include <iostream> 
2 | #include <vector> 

3 |#include <fstream> 

4 |using namespace std; 


s |#include <boost/timer.hpp> 


7 |// for sophus 
s | *include <sophus/se3.h> 





t=0 t=30 t = 100 





t — 150 


图 13-5 ”数据 集 图 示 。 


using Sophus::SE3; 


// for eigen 


include <Eigen/Core> 


#include <Eigen/Geometry> 


using namespace Eigen; 


#include <opencv2/core/core.hpp> 


#include <opencv2/highgui/highgui .hpp> 


#include <opencv2/imgproc/imgproc.hpp> 


using namespace cv; 


RRR OOO OR ORR IO OR OR AK KK 
* 本 程序 演示 了 单 目 相机 在 已 知 轨迹 下 的 稠密 深度 估计 
* 使 用 极 线 搜索 + NCC 匹配 的 方式 ， 与 本 书 13.2 节 对 应 
* 请 注意 ， 本 程序 并 不 完美 ， 你 完全 可 以 改进 它 。 
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// parameters 


const 
const 
const 
const 


const 


int boarder = 20; 
int width = 640; 

int height = 480; 
double fx = 481.2f; 
double fy = -480.0f; 


// BRR 
// 宽度 
// 高 度 
// 相机 内 参 


const double cx 


const double cy 


319.5f; 
239.5f; 


const int ncc window size = 2; // NCC 取 的 窗口 半 宽 度 


const int ncc area - (2*ncc. 


const double min cov = 0.1; 


const double max cov 


// 重要 的 


10; 


函数 


// 从 REMODE 数据 集 读 取 数 据 
bool readDatasetFiles( 


const 


string& path, 


window size-*i)*(2xncc window size*1); // NCCR n QA 


// WS PE: 最 小 方差 
// RRA RL: 最 大 方差 


vector<string>& color_image_files, 


vector<SE3>& poses 


23 


// 根据 新 的 图 像 更 新 深度 估计 
bool update( 


const 


Mat& ref, 


const Mat& curr, 


const SE3& T_C_R, 
Mat& depth, 
Mat& depth_cov 

); 

// 极 线 搜索 

bool epipolarSearch( 
const Mat& ref, 


const 
const 
const 
const 


const 


Mat& curr, 

SE3 T C R, 
Vector2d& pt ref, 
double& depth mu, 
double& depth cov, 


Vector2d& pt curr 


)3 


// 更 新 深度 滤波 器 
bool updateDepthFilter( 


const 
const 


const 


Vector2d& pt ref, 
Vector2dg pt curr, 
SE3& T C R, 


Mat& depth, 
Mat& depth cov 


80 


81 


// 计算 NCC 评分 


double NCC( const Mat& ref, const Mat& curr, const Vector2d& pt ref, const Vector2d 


& pt curr ); 


// 双 线 性 灰 度 插值 


inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt ) { 
uchar* d = & img.data[ int(pt(1,0))*img.step*int(pt(0,0)) ]; 


double xx = pt(0,0) - floor(pt(0,0)); 

double yy = pt(1,0) - floor(pt(1,0)); 

return (( 1-xx ) * ( 1-yy ) * double(d[0]) + 
xx* ( 1-yy ) * double(d[1]) + 
( 1-xx ) *yy* double(d[img.step]) + 


xx*yy*double(d[img.step*1]))/255.0; 


// 一 些小 工具 
// 显示 估计 的 深度 图 
bool plotDepth( const Mat& depth ); 


// 像素 到 相机 坐标 系 
inline Vector3d px2cam ( const Vector2d px ) { 
return Vector3d ( 
(px(0,0) = cx)/fx, 
(px(1,0) - cy)/fy, 
1 
) ; 


// 相机 坐标 系 到 像素 
inline Vector2d cam2px ( const Vector3d p cam ) { 
return Vector2d ( 
p.cam(0,0)*fx/p cam(2,0) + cx, 
p.cam(1,0)*fy/p cam(2,0) * cy 
); 


// 检测 一 个 点 是 否 在 图 像 边框 内 
inline bool inside( const Vector2d& pt ) 1 
return pt(0,0) >= boarder && pt(1,0)>=boarder 


&& pt(0,0)+boarder<width && pt(1,0)+boarder<=height ; 


int main( int argc, char** argv ) 


{ 


124 if ( argc != 2) 


125 1 

126 cout««"Usage: dense mapping path to test dataset"««endl; 

127 return -1; 

128 } 

129 

130 // 从 数据 集 读 取 数据 

131 vector<string> color image files; 

132 Vector<SE3> poses, TWC; 

133 bool ret = readDatasetFiles( argv[1], color image files, poses TWC ); 
134 if ( ret--false ) 

135 1 

136 cout««"Reading image files failed!"««endl; 

137 return -1; 

138 } 

139 cout<<"read total "<<color_image_files.size()<<" files."««endl; 
140 

141 // 第 一 张 图 

142 Mat ref = imread( color_image_files[0], 0 ); // gray-scale image 
143 SE3 pose ref TWC = poses TWC[O]; 

144 double init depth = 3.0; // 深度 初始 值 

145 double init_cov2 = 3.0; // 方差 初始 值 

146 Mat depth( height, width, CV 64F, init depth ); // 深度 图 
147 Mat depth cov( height, width, CV 64F, init cov2 ); // 深度 图 方差 
148 

149 for ( int index-1; index<color_image_files.size(); index++ ) 

150 1 

151 cout<<"*** loop "<<index<<" ***"<<end1; 

152 Mat curr = imread( color image files[index], 0 ); 

153 if (curr.data -- nullptr) continue; 

154 SE3 pose curr TWC = poses TWC[index]; 

155 SE3 pose T C. R = pose curr TWC.inverse() * pose ref TWC; 

156 // 坐标 转换 关系 : TCW*TWR-TCR 

157 update( ref, curr, pose T C R, depth, depth, cov ) ; 

158 plotDepth( depth ); 

159 imshow("image", curr); 

160 waitKey(1); 

161 } 

162 

163 

164 return O0; 

165 |} 


166 
i; | // 对 整个 深度 图 进行 更 新 
168 | bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& 


depth cov ) 
1 
#pragma omp parallel for 
for ( int x=boarder; x<width-boarder; x++ ) 
#pragma omp parallel for 
for ( int y=boarder; y<height-boarder; y** ) 
1 
// 遍历 每 个 像素 
if ( depth_cov.ptr<double>(y) [x] < min. cov 
|| depth_cov.ptr<double>(y) [x] > max cov ) // RM CK eX 3X, Rik 
continue; 
// 在 极 线 上 搜索 (m,y) 的 匹配 
Vector2d pt_curr; 
bool ret = epipolarSearch ( 
ref, 
curr, 
TCR, 
Vector2d(x,y), 
depth. ptr<double>(y) [x], 
sqrt (depth_cov.ptr<double>(y) [x]), 
pt_curr 


)i 


if ( ret == false ) // 匹配 失败 


continue; 


// 取消 该 注释 以 显示 匹配 
// showEpipolarMatch( ref, curr, Vector2d(z,y), pt curr ); 


// 匹配 成 功 ， 更 新 深度 图 
updateDepthFilter( Vector2d(x,y), pt curr, T C R, depth, depth cov ); 


// 极 线 搜索 
// 方法 见 本 书 13.2, 13.3 两 节 
bool epipolarSearch( 
const Mat& ref, const Mat& curr, 
const SE3& T_C_R, const Vector2d& pt ref, 
const double& depth mu, const double& depth cov, 
Vector2d& pt curr 


Vector3d f ref = px2cam( pt ref ); 


f ref.normalize(); 


Vector3d P ref = f ref*depth mu; // 参考 帧 的 P 向 量 


Vector2d px mean curr = cam2px( T.C R*P ref ); // 按 深度 均值 投影 的 像素 

double d min = depth mu-3*depth cov, d max = depth mu-*3*depth cov; 

if ( d min«0.1 ) d min = 0.1; 

Vector2d px min curr = cam2px( T C R*(f ref*d min) );  // 按 最 小 深度 投影 的 像素 
Vector2d px max curr = cam2px( T.C R*(f ref*d max) ); // 按 最 大 深度 投影 的 像素 


Vector2d epipolar line = px max curr - px min curr; // MA (线段 形式 ) 
Vector2d epipolar direction - epipolar line; // 极 线 方向 
epipolar direction.normalize(); 

double half length = 0.5*epipolar_line.norm(); // 极 线 线段 的 半 长 度 

if ( half_length>100 ) half length = 100; // 我 们 不 希望 搜索 太 多 东西 


// 取消 此 名 注释 以 显示 极 线 (AK) 


// showEpipolarLine( ref, curr, pt ref, poc min curr, pz_maz_curr ); 


// 在 极 线 上 搜索 ， 以 深度 均值 点 为 中 心 ， 左 右 各 取 半 长 度 
double best ncc = -1.0; 
Vector2d best px curr; 
for ( double l--half length; l«-half length; 1+=0.7 ) // l+=sgrt (2) 
1 
Vector2d px curr = px mean curr + l*epipolar direction; // 待 匹 配点 
if ( 'inside(px curr) ) 
continue; 
// 计算 待 匹 配点 与 参考 帧 的 NCC 
double ncc = NCC( ref, curr, pt ref, px curr ); 


if ( ncc»best ncc ) 


1 
best ncc = ncc; 
best px curr - px curr; 
} 
} 
if ( best ncc < 0.85f ) // 只 相信 NCC 很 高 的 匹配 


return false; 
pt_curr = best px curr; 


return true; 


double NCC ( 


const Mat ref, const Mat& curr, 


const Vector2d& pt ref, const Vector2dg pt curr 


// 零 均值 - 归 一 化 互相 关 


// 3E 3 th 

double mean ref = O0, mean curr = 0; 

vector<double> values ref, values curr; // 参考 帧 和 当前 帧 的 均值 
for ( int x--ncc window size; x«-ncc window size; x++ ) 


for ( int y--ncc window size; y«-ncc window size; y++ ) 


1 
double value ref = double(ref.ptr<uchar>( int(y*pt ref(1,0)) )[ int(x+ 
pt .ref(0,0)) ])/255.0; 
mean ref += value ref; 
double value curr = getBilinearInterpolatedValue( curr, pt curr* 
Vector2d(x,y) ); 
mean curr -*- value curr; 
values ref.push back(value ref); 
values curr.push back(value, curr); 
} 


mean_ref /= ncc_area; 


mean_curr /= ncc_area; 


// 计算 Zero mean NCC 
double numerator = 0, demoniatori = 0, demoniator2 = 0; 


for ( int i=0; i<values_ref.size(); i++ ) 


{ 
double n = (values ref[i]-mean ref) * (values_curr[i]-mean_curr) ; 
numerator += n; 
demoniatori += (values ref[i]-mean ref)*(values ref[i]-mean ref); 
demoniator2 *- (values curr[i]-mean curr)*(values curr[i]-mean curr); 

) 

return numerator / sqrt( demoniatori*demoniator2*1e-10 ); // BELDA EME 

} 


bool updateDepthFilter( 
const Vector2d& pt_ref, 
const Vector2d& pt_curr, 
const SE3& T C R, 
Mat& depth, 
Mat& depth cov 


// 用 三 角 化 计算 深度 
SE3 T_R_C = TC R.inverse(); 
Vector3d f_ref = px2cam( pt_ref ); 


f ref.normalize(); 


Vector3d f curr = px2cam( pt curr ); 


f curr: 


normalize(); 


// 方程 参照 本 书 第 7 讲 三 角 化 一 节 
Vector3d t = T R C.translation(); 


Vector3d f2 = T R C.rotation matrix() * f curr; 
Vector2d b = Vector2d ( t.dot ( f ref ), t.dot ( f2 ) ); 


double 
A [0] 
A[2] 
A[1] 
A[3] 
double 


A [4]; 

f ref.dot ( f ref ); 

f ref.dot ( f2 ); 

~A I2] ; 

- f2.dot ( £2 ); 

d = A[0]*A[3] -A[1] *A[2] ; 


Vector2d lambdavec - 
Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 D), 


-A[2] * b ( 0,0) + A[O] * b ( 1,0 )) /di 


Vector3d xm = lambdavec ( 0,0 ) * f ref; 


Vector3d xn 
Vector3d d esti = ( xm*xn ) / 2.0; 


double 


t * lambdavec ( 1,0 ) * f2; 


// 深度 值 


depth estimation = d esti.norm(); 


// 计算 不 确定 性 (以 一 个 像素 为 误差 ) 
Vector3d p = f ref*depth estimation; 


Vector3d a =p - t; 


double 
double 
double 
double 
double 
double 
double 
double 
double 


t norm = t.norm(); 

a norm = a.norm(); 

alpha = acos( f ref.dot(t)/t norm ); 

beta = acos( -a.dot(t)/(a norm*t norm)); 

beta prime = beta + atan(1/fx); 

gamma = M PI - alpha - beta prime; 

p.prime - t norm * sin(beta prime) / sin(gamma); 
d cov = p prime - depth estimation; 


d cov2 = d cov*d cov; 


// 高 斯 融合 


double 
double 


double 
double 


depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt ref(0,0)) ] 
depth cov.ptr«double»( int(pt ref(1,0)) )[ int(pt_ref(0,0)) ] 


return 


mu = depth.ptr<double>( int(pt ref(1,0)) )[ int(pt_ref(0,0)) ]; 
sigma2 = depth cov.ptr«double»( int(pt ref(1,0)) )[ int(pt ref(0,0)) 1]; 


mu fuse = (d cov2*mu*sigma2*depth estimation) / ( sigma2*d, cov2); 


sigma fuse2 - ( sigma2 * d cov2 ) / ( sigma2 * d cov2 ); 


true; 


// 三 角 化 算得 的 深度 向 量 


sigma_fuse2; 


46 |} 


47 
4 | // 其 他 次 要 的 函数 略 





如 采 恋 者 理解 了 上 一 下 内容 ， 相 信访 懂 此 处 源 代 人 码 亦 不 是 难事 。 尽 
管 如 此 ， 我 们 对 几 个 关键 男 数 稍 做 说 明 : 

1.main 函 数 非 党 简单。 它 只 人 负 贡 从 数据 集中 读 取 图 像 ， 然 后 交 给 
updates aX, TYREE GET E KT - 

2.updaterK Zi rH, RADEN SEMENAR, TE HA 
RRRA, crBeULEG E, MAHR IL CY R E RE RAET e 


3.] 2 18 RREAK EETA, BERK EA f E 
P: 因为 假设 次 度 值 服 从 高 斯 分 布 ， 我 们 融 以 均值 为 中 心 ， 左 右 符 取 芋 
3o ”作为 半径 ， 然 后 在 当前 帧 中 寻找 极 线 的 投影 。 然 后 ， 通 历 此 极 线 上 
WBA CRB s 的 近似 值 0.7) ， 寻 找 NCC 最 高 的 点 作为 匹配 点 。 如 
果 最 高 的 NCC 也 低 于 靖 值 (这 里 取 0.85) ， 则 认为 匹配 失败 。 


4.NCC 的 计算 使 用 了 去 均值 化 后 的 做 法 ， 即 对 于 图 像 其 A,B ， 取 : 


© (A(63) — Ali, j)) (BG, 3) - BU, j)) 





NEG, DA, He =i (55) 


5. 三 角 化 的 计算 方式 与 7.5 贡 一 致 ， 不 确定 性 的 计算 与 高 斯 融合 方法 
和 上 一 节 一 致 。 

虽然 程序 有 些 长 ， 相 信 读 者 根据 上 面 的 提示 能 读 恒 。 下 面 我 们 来 看 
EER SE Psd AT BUR o 

实验 结果 

编译 此 程序 后 ， 以 数据 集 目 录 作 为 参数 运行 之 中: 


1 |$ build/dense mapping -/dataset/test data 
2 |read total 202 files. 
3 |*** loop 1 *** 


4 | **x loop 2 **x 





程序 输出 的 信息 比较 简洁 ， 仅 显示 了 迭代 次 数 、 当 前 图 像 和 深度 
图 。 关 于 深度 图 ， 我 们 显示 的 是 深度 值 乘 以 0.4 后 的 结果 一 —19. 8b xe 2 
AHA (BUR A1.0) 的 深度 约 2.5 米 ， 关 色 越 深 表示 深度 值 越 小 ， 也 残 是 
物体 离 我 们 越 近 。 如 果实 际 运行 了 程序 ， 应 该 会 发 现 深 上 度 估计 是 一 个 动 
态 的 过 程 一 从 一 个 不 怎么 确定 的 初始 值 逐 渐 收 敛 到 稳定 值 的 过 程 。 我 
们 的 初始 值 使 用 了 均值 和 方差 均 为 3.0 的 分 布 。 当 然 你 也 可 以 修改 初始 
分 布 ， 看 看 对 结果 会 产生 怎样 的 影 啊 。 


从 图 13-6 可 以 及 现 ， 当 运 代 次 数 超过 一 定 值 之 后 ， 深 度 图 壕 于 稳 
定 ， 丰 天 对 新 的 数据 产生 改变 。 观 内 稳 定之 后 的 深度 图 ， 我 们 友 现 大 致 
可 以 看 出 地 板 和 时 子 的 区 别 ， 而 果 上 的 物体 深度 则 接近 于 果子 。 整 个 全 
计 大 部 分 是 正确 的 ， 但 也 存在 看 大 量 错误 信 计 。 它 们 表现 为 深度 图 中 与 
周围 数据 不 一 致 的 地 方 ， 为 过 六 或 过 小 的 估计 。 此 外 ， 位 于 边缘 处 的 地 
JH. Pe ske a m SU. MAINA SEEM Ta To Sr 
EAA, SA A Ak AREE ED AGB ot ce EB, (ACA IA BI FOUR BA 
朱 。 我 们 将 在 下 一 地 分 析 这 些 情 况 的 出 现 原 因 ， 并 讨论 有 哪些 可 以 改进 
的 地 方 。 








图 13-6 ”演示 程序 
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13.4 实验 分 析 与 讨论 


上 一 节 我 们 演示 了 移动 单 目 相机 的 稠密 建 图 ， 估 计 了 参考 帧 的 每 个 
像素 深度 。 我 们 的 代码 是 相对 简单 直接 的 ， 没 有 使 用 许多 的 技巧 
Crick) ， 因 此 出 现 了 实际 工程 中 常见 的 情形 一 简单 的 往往 并 不 是 最 
txt. 

由 于 真实 数据 的 复杂 性 ， 能 够 在 实际 环境 下 工作 的 程序 ， 往 往 需要 
周密 的 考虑 和 大 量 的 工程 技巧 ， 这 使 得 每 种 实际 可 行 的 代码 都 极其 复杂 
它们 很 难 向 初学 者 解释 清楚 ， 所 以 我 们 只 好 使 用 不 那么 有 效 ， 但 相 
对 易 读 易 写 的 实现 方式 。 我 们 当然 可 以 提出 若干 种 对 演示 程序 加 以 改进 
的 意见 ， 不 过 这 里 并 不 打算 把 已 经 改 好 的 〈 非 常 复杂 的 ) 程序 直接 呈现 
给 读者 ， 

下 面 我 们 对 上 一 节 实验 的 结果 进行 初步 分 析 。 我 们 将 从 计算 机 视觉 
和 滤波 器 两 个 角度 来 分 析 演示 实验 的 结 
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与 个 ， 依 赖 于 图 像 块 是 否 具 有 区 分 度 。 显 然 ， 如 果 图 像 块 仅 是 一 片 黑 或 
者 一 片 白 ， 缺 少 有 效 的 信息 ， 那 么 在 NCC 计 算 中 我 们 就 很 可 能 错误 地 将 
它 与 周围 的 某 块 像素 给 匹配 起 来 。 请 读者 观察 演示 程序 中 的 打印 机 表 
面 。 由 于 它 是 均 勾 的 白色 ， 非 帝 容 易 引 起 误 匹 配 ， 因 此 打印 机 表面 的 深 
度 信 息 多 半 是 不 正确 的 一 一 示例 程序 的 空间 表面 出 现 了 明显 不 该 有 的 条 
纹 状 深度 估计 ， 而 根据 我 们 的 直观 想象 ， 打 印 机 表面 肯定 是 光滑 的 。 

这 里 罕 涉 到 了 一 个 问题 ， 该 问题 在 直接 法 中 我 们 已 经 见 过 一 次 。 在 
进行 块 匹 配 〈 和 NCC 的 计算 ) 时 ， 我 们 必须 假设 小 块 不 变 ， 人 然后 将 该 小 
块 与 其 他 小 块 进行 对 比 。 这 时 ， 有 明显 梯度 ”的 小 块 将 具有 民 好 的 区 分 
度 ， 不 易 引 起 误 匹 配 。 对 于 梯度 不 明显 的 像素 ， 由 于 在 块 匹 配 时 没有 
区 分 性 ， 所 以 我 们 将 难以 有 效 地 估计 其 深度 。 反 之 ， 像 素 梯度 比较 明显 
的 地 方 ， 我 们 得 到 的 深度 信息 也 相对 人 准确， 例如 桌面 上 的 杂志 、 电 话 等 
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见 的 问题 : 对 物体 纹理 的 依赖 性 。 该 问题 在 双 目 视觉 中 也 极其 常见 ， 
体现 了 立体 视觉 的 重建 质量 十 分 依赖 于 环境 纹理 。 


我 们 的 狐 示 程序 刻意 使 用 了 纹理 较 好 的 环境 ， 例 如 ， 像 棋盘 格 一 般 
的 地 板 ， 市 有 木 纹 的 条 面 ， 等 等 ， 因 此 能 得 到 一 个 看 似 不 错 的 结束 。 然 
而 在 实际 中 ， 像 场面 、 光 请 物体 表面 等 涡 度 均匀 的 地 方 将 经 负 出 现 ， 影 
啊 我 们 对 乱 的 深度 佑 计 。 从 未 种 角度 来 说 ， 访 问题 是 无 法 在 现 有 的 算法 
法 程 上 加 以 改进 并 解决 的 ”一 一 如 来 我 们 依然 只 关心 东 个 像 系 周围 的 邻 
域 《小 块 ) 的 话 。 


进一步 讨论 像 际 梯度 问题 ， 我 们 还 会 肥 现 像 系 梯度 和 极 线 之 间 的 联 
系 。 文 草 [59] 详 细 讨 论 过 它们 的 关系 ， 不 过 在 我 们 的 泗 示 程序 里 也 有 下 
观 的 体现 。 


以 图 13-7 为 例 ， 我 们 举 两 种 比较 极 病 的 迟 况 : 像 系 梯度 平行 于 极 线 
方 同 ， 以 及 垂直 于 极 线 方 同 。 先 来 看 牌 耳 的 情况 。 在 垂 卫 的 例子 里 ， 即 
使 小 块 有 明显 标 度 ,但 当 我 们 沿 看 极 线 去 做 块 匹 配 时 ， 会 友 现 匹配 程度 
祁 征 一 样 的 ， 因 此 得 不 到 有 效 的 匹配 。 上 反之， 在 平行 的 例子 里 ， 我 们 能 
够 精确 地 确定 匹配 度 最 高 点 出 现在 何 处 。 而 实际 当中 ， 柳 度 与 极 线 的 情 
况 很 可 能 介 于 二 者 之 间 : 既 不 是 完全 竺 直 处 不 是 完全 平行 。 这 时 ， 我 们 
说 ， 当 像 妹 梯度 与 极 线 认 角 较 大 时 ， 极 线 匹 配 的 不 确定 性 大 ;而 当 炎 角 
较 小 时 ， 匹 配 的 不 确定 性 变 小 。 而 在 演示 程序 中 ， 我 们 统一 地 把 这 些 情 
况 部 当成 一 个 像 系 的 误 雄 ， 实 际 是 不 够 精细 的 。 考 虑 到 极 线 与 像 系 梯度 
的 关系 后 ， 应 该 使 用 更 精确 的 不 确定 性 模型 。 具 体 的 调整 和 改进 留 作 习 
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13.4.2 wire 


从 男 一 个 角度 看 ， 我 们 不 妨 问 : 把 像素 深度 假设 成 局 斯 分 布 是 售 合 
ae? 这 里 关系 到 一 个 参数 化 的 问题 (Parameterization) 。 


在 十 面 的 内 容 中 ， 我 们 经 常用 一 个 点 的 世界 坐标 x,y,z 三 个 量 来 手 述 
它 ， 这 是 一 种 参数 化 形式 。 我 们 认为 x,y,z 三 个 量 虱 是 随机 的 ， 它 们 服从 
(三 维 的 ) 噩 斯 分 布 。 然 而 ， 本 讲 使 用 了 图 像 坐标 u,v 和 深度 值 d 来 拍 述 
AT ARIAL A CBE) 。 我 们 认为 wv 个 动 ， 而 d 服从 一 维 的 ) 局 
WI. AKEN d nini 那么 我 们 要 问 : 这 两 种 参数 化 形式 有 
什么 不 同 吗 ? 我 们 是 否 也 能 假设 u,v 服从 高 斯 分 布 ， 从 而 形成 男 一 种 参 
数 化 形式 呢 ? 


不 同 的 参数 化 形式 ， 实 际 都 接 述 了 同一 个 量 ， 也 就 是 某 个 三 维 空间 
点 。 考 虑 到 当 我 们 在 相机 看 到 某 个 点 时 ， 它 的 图 像 坐标 u,v 是 比较 确定 
的 SI ， 而 深度 值 d 则 是 非常 不 确定 的 。 此 时， 大 用 世界 坐标 x,y,z 描述 这 
个 点 ， 那 么 根据 相机 当前 的 位 姿 ，x,y,z 三 个 量 之 间 可 能 存在 明显 的 相关 
性 。 反 映 在 协 方 老 窍 阵 中 ， 表 现 为 非 对 角 元 陛 不 为 零 。 而 如 果 用 uv,d 2 
数 化 一 个 点 ， 那 么 它 的 wuv Md 至少 是 近似 独立 的 ， 甚 至 我 们 还 能 认 
为 uyv 也 是 独立 的 一 从 而 它 的 协 方 甜 官 阵 近 似 为 对 角 阵 ， 更 为 傈 洛 。 

逆 深 度 (Inverse depth) 是 近年 来 SLAM 人 研究 中 出 现 的 一 种 广泛 使 用 
的 参数 化 拉 巧 上 1425 V CEN Bee, EVRA AE re T2) 48: d 
~N (uo ? )。 然 而 这 样 做 合 不 合理 呢 ? 深度 真 的 近似 于 一 个 高 斯 分 布 
n? 仔细 想 想 ， 深 度 的 正 态 分布 确 实 存在 一 些 问题 : 

1. 我 们 实际 想 表 达 的 是 : 这 个 场景 深度 大 概 是 5 一 10 米 ， 可 能 有 一 
些 更 远 的 点 ， 但 近 处 肯定 不 会 小 于 相机 焦距 〈 或 认为 深度 不 会 小 于 
0) 。 这 个 分 布 并 不 是 像 高 斯 分 布 那样 ， 形 成 一 个 对 称 的 形状 。 扰 的 尾 
部 可 能 稍 长 ， 而 负数 区 域 则 为 零 。 

2. 在 一 些 室外 应 用 中 ， 可 能 存在 距离 非常 远 ， 力 至 无 穷 远 处 的 点 。 
我 们 的 初始 值 中 难以 闻 羡 这 些 点 ， 并 且 用 高 斯 分 布 摘 述 它们 会 有 一 些 数 
值 计 算 上 的 困难 。 

于 是 ， 地 深度 应 运 而 生 。 人 们 在 仿真 中 发 现 ， 假 设 深度 的 倒数 ， 也 
Wee WTR ， 为 局 斯 分 布 是 比较 有 效 的 出 。 随 后 ， 在 实际 应 用 中 ， 逆 
深度 也 具有 更 好 的 数值 稳定 性 ， 从 而 逐渐 成 为 一 种 通用 的 技巧 ， 存 在 于 
现 有 SLAM 方 案 中 的 标准 做 法 中 B65773] 。 

把 演示 程序 从 正 深度 改 成 刻 深度 亦 不 复 森 。 只 要 在 前 面 出 现 深 友 的 
推导 中 ， 将 qd ARARE + 即 可 。 我 们 亦 把 这 个 改动 留 作 习题 ， 交 给 
证 者 完成 。 


13.4.3 B IRIS AR 
在 块 匹 配 之 前 ， 做 一 次 图 像 到 图 像 间 的 变换 亦 是 一 种 常见 的 预 处 理 


方式 。 这 是 因为 ， 我 们 假设 了 图 像 小 块 在 相册 运动 时 你 持 不 变 ， 而 这 个 
假设 在 相机 平移 时 《示例 数据 集 基 本 都 是 这 样 的 例 于 ) BES TRE MIL 


但 当 相机 发 生 明显 的 旋转 时 ， 就 难以 继续 保持 了 。 特 别 地 ， 当 相机 绕 光 
心 旋转 时 ， 一 块 下 黑 上 白 的 图 像 可 能 会 变 成 一 个 上 黑 下 和 白 的 图 像 块 ， 导 
致 相 关 性 直接 变 成 了 负数 〈 尽 管 仍 然 是 同样 一 个 块 ) 。 
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与 真实 的 三 维 点 世界 坐标 Py 有 以 下 天 系 : 
drPr = K (RrwPw + trw). (13.13) 
类 似 地 ， 对 于 当前 帧 ， 亦 有 P,, 在 它 上 边 的 投影 ， 记 作 P。_: 
do Po = K (Rcw Pw + tew). (13.14) 
代入 并 消去 P,, — EIAS RZ IRIBS TR ER DR : 
do Po = dgK RewRawK | Pg + Ktcw — K Row Ri Ktgw. (13.15) 


当知 道 d ,Pr 时 ， 可 以 计算 出 Po 的 投影 位 置 。 此 时 ， 再 给 Ps 两 个 分 
量 各 一 个 增 量 du,dv ， 融 可 以 求 得 Pe 的 增 量 du, dv, 。 通 过 这 种 方式 ， 算 
出 在 局 部 范围 内 参考 帧 和 当前 帆 图 像 坐 标 变 换 的 一 个 线性 关系 构成 仿 射 


变换 ; 
d ue de de du 
dv, = à iu. 1 (13.16) 
根据 仿 射 变换 和 矩阵， 我们 可 以 把 当前 帧 〈 或 参考 帧 ) 的 像素 进行 变 
换 后 再 进行 块 匹 配 ， 以 期 获得 对 旋转 更 好 的 效果 。 
13.4.4 FTW: 效率 的 问题 


在 实验 当中 我 们 也 看 到 ， 币 密 深度 图 的 估计 非 第 费时 ， 这 是 因为 我 
们 要 估计 的 点 从 原先 的 数目 个 特征 点 一 下 子 变 成 了 几 十 万 个 像素 点 ， 即 








使 现在 主流 的 CPU 也 无 法 实时 地 计算 那样 庞大 的 数量 。 不 过 ， 该 问题 亦 
有 另 一 个 性 质 ， 这 几 十 万 个 像素 点 的 深度 估计 是 彼此 无 关 的 ! 这 使 并 行 
化 有 了 用 武之 地 ， 


在 示例 程序 中 ， 我 们 在 一 个 二 重 循环 里 通 历 了 所 有 休 系 ， 并 逐个 对 
它们 进行 极 线 搜索 。 当 我 们 使 用 CPU 时 ， 这 个 过 程 是 串 行 进行 的 ;必须 
症 上 一 个 像 系 计算 完毕 后 ， 再 计算 下 一 个 像 系 。 然 而 实际 上 ， 下 一 个 像 
系 完 全 没有 必要 等 待 上 一 个 像 系 的 计算 结束 ， 因 为 它们 之 间 并 没有 明 最 
的 联系 ， 所 以 我 们 可 以 用 多 个 线程 ， 分 别 计 算 每 个 像 系 ， 然 后 将 结果 统 
一 起 来 。 理 论 上 ， 如 琳 我 们 有 30 万 个 线程 ， 那 么 该 问题 的 计算 时 间 和 计 
算 一 个 像素 是 一 样 的 。 


GPU 的 并 行 计 算 架 构 非 常 适合 这 样 的 问题 ， 因 此 ， 在 单 双 和 双 目 的 
稠密 重建 中 ， 经 负 看 到 利用 GPU 进行 并 行 加 速 的 方式 。 当 然 ， 本 书 不 准 
备 涉 及 GPU 编程 ， 所 以 我 们 在 这 里 仅 指 出 利用 GPU 加 速 的 可 能 性 ， 有 具体 
实践 留 给 读者 作为 验证 。 根 据 一 些 类 似 的 工作 ， 利 用 GPU 的 稠密 深度 佑 
计 是 可 以 在 主流 GPU 上 实时 化 的 。 


13.4.5 ”其 他 的 改进 


事实 上 ， 我 们 还 能 提出 许多 对 本 例 程 进行 改进 的 方案 ， 例 如 : 


1. 现 在 各 像 系 完全 是 独立 计算 的 ， 可 能 存在 这 个 像 系 深度 很 小 ， 边 
上 一 个 义 很 大 的 情况 。 我 们 可 以 假设 深度 图 中 相 邻 的 深度 变化 不 会 太 
大 ， 从 而 给 深度 信 计 加 上 了 空间 正则 项 。 这 种 做 法 会 使 得 到 的 深度 图 更 
加 平滑 。 


2. 我 们 没有 显 式 地 处 理 外 点 COutlier) 的 情况 。 事 实 上 ， 由 于 让 
挡 、 光 照 、 运 动 模糊 等 各 种 因 系 的 影响 ， 不 可 能 对 每 个 像素 都 能 保持 成 
功 匹 配 。 而 演示 程序 的 做 法 中 ， 只 要 NCC 大 于 一 定 值 ， 就 认为 出 现 了 成 
功 的 匹配 ， 没 有 考虑 到 错误 匹配 的 情况 。 


处 理 错 误区 配 亦 有 右 干 种 方式 。 例 如 ， 文 献 [112] 提 出 的 均匀 -局 斯 
混合 分 布下 的 深度 小 流血， 显 式 地 将 内 扣 与 外 后进 行 区 别 并 进行 概率 建 
模 ， 能 够 较 好 地 处 理 外 后 数据 。 然 而 这 种 类 型 的 滤波 如 理 论 较 为 复 森 ， 
本 书 不 想 过 多 涉及 ， 读 痢 可 以 阅读 原 妈 论 文 。 


从 上 面 的 讨论 可 以 看 出 ， 存 在 着 许多 可 能 的 改进 方案 。 如 末 我 们 细 
致 地 改进 每 一 步 的 做 法 ， 了 最 后 是 有 和 希望 得 到 一 个 民 好 的 稠密 建 图 的 方案 
的 。 然 而， 正如 我 们 所 讨论 的 ， 有 一 些 问题 存在 理论 上 的 困难 ”， 例 如 
对 环境 纹理 的 依赖 ， 义 如 像 系 柳 度 与 极 线 方 回 的 关联 (以 及 平行 的 情 
况 ) 。 这 些 问 题 人 很 难 通过 调整 代码 实现 来 解决 o MA, ES ERN 
止 ， 尽 管 双 目 和 移动 单 目 能 够 建立 稠密 的 地 图 ， 我 们 通 利 认为 它们 过 于 
依赖 于 环境 纹理 和 光照 ， 不 够 可 徘 。 
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除了 使 用 单 目 和 双 目 进行 稠密 重建 之 外 ， 在 适用 范围 内 ，RGB-D 相 
机 是 一 种 更 好 的 选择 。 在 上 一 讲 中 详细 讨论 的 深度 估计 问题 ， 在 RGB-D 
相机 中 可 以 完全 通过 传感器 中 硬件 测量 得 到 ， 无 须 消耗 大 量 的 计算 资源 
来 估计 。 并 且 ，RGB-D 的 结构 光 或 飞 时 原理 ， 保 证 了 深度 数据 对 纹理 的 
无 关 性 。 即 使 面 对 纯 色 的 物体 ， 只 要 它 能 够 反射 光 ， 我 们 束 能 测量 到 它 
的 深度 。 这 亦 是 RGB-D 传 感 器 的 一 大 优势 。 


利用 RGB-D 进 行 稠密 建 钢 是 相对 容易 的 。 不 过 ， 根 据 地 图 形 陈 不 
E, RTE FPA ERAT SU. RAW. mA IE, Be 
是 根据 估算 的 相机 位 姿 ， 将 RGB-D 数 据 转 化 为 点 云 (Point Cloud) ， 然 
后 进行 拼接 ， 最 后 得 到 一 个 由 离散 的 点 组 成 的 点 云 地 图 (Point Cloud 
Map) 。 在 此 基础 上 ， 如 采 我 们 对 外 观 有 进一步 的 要 求 ， 硕 望 估计 物体 
的 表面 ， 可 以 使 用 三 角 网 格 (Mesh) 、 面 片 〈Surfel) 进行 建 图 。 另 一 
方面 ， 如 采 和 硕 望 知道 地 图 的 障 但 物 信息 并 在 地 图 上 导航 ， 尔 可 通过 体系 
(Voxel) 建立 占据 网 格 地 图 (Occupancy Map) . 


我 们 似乎 引入 了 很 多 新 概念 。 请 读者 不 要 着 急 ， 我 们 将 慢 慢 地 逐一 
加 以 介绍 。 对 于 部 分 适合 进行 实验 的 ， 我 们 亦 会 像 往 常 一 样 ， 提 供奉 干 
个 演示 程序 。 由 于 RGB-D 建 图 罕 涉 到 的 理论 知识 并 不 很 多 ， 所 以 下 面 几 
节 就 直接 以 实践 部 分 来 介绍 了 。GPU 建 图 超出 了 本 书 的 范围 ， 我 们 就 简 
单 讲 解 其 原理 ， 不 做 演示 。 


13.5.1 Xk: 点 云 地 图 


Hu. 我们 来 讲解 最 简单 的 点 云 地 图 。 所 谓 点 云 ， 束 是 由 一 组 离散 
的 点 表示 的 地 图 。 最 基本 的 点 包含 x,y,z 三 维 坐标 ， 也 可 以 带 有 r,g,b 的 彩 
色 信 息 。 由 于 RGB-D 相 机 提供 了 彩色 图 和 深度 图 ， 很 容易 根据 相机 内 参 
来 计算 RGB-D 扩 云 。 如 来 通过 条 种 手段 ， 得 到 了 相机 的 位 姿 ， 那 么 只 要 
和 卫 接 把 点 云 进行 加 和 ， 束 可 以 获得 全 局 的 点 云 。 在 本 书 的 5.4 订 ， 兽 给 
出 了 一 个 通过 相机 内 外 参 拼接 点 云 的 例子 。 不 过 ， 那 个 例子 主要 是 为 了 


让 读者 理解 相机 的 内 外 参 ， 而 在 实际 建 图 当中 ， 我 们 还 会 对 点 云 加 一 些 
滤波 处 理 ， 以 获得 更 好 的 视 沉 效果 。 在 本 程序 中 ， 我 们 主要 使 用 两 种 泪 


EE 
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于 部 分 代码 与 之 前 的 相同 ， 我 们 主要 看 改变 的 部 分 ) ， 
四 slambook/ch13/dense RGBD/pointcloud mapping.cpp Cr EX) 


~ A A —-— A A 


int main( int argc, char** argv ) 


{ 


// 图 像 读 取 部 分 略 

// 足 义 点 云 使 用 的 格式 : 这 里 用 的 是 XYZRGB 
typedef pcl::PointXYZRGB PointT; 

typedef pcl::PointCloud<PointT> PointCloud; 


// 新 建 一 个 点 云 
PointCloud: :Ptr pointCloud( new PointCloud ); 
for ( int i20; i<5; i++ ) 
1 
PointCloud::Ptr current( new PointCloud ); 
cout««" 42 4& ARP: "<<it+1<<endl; 
cv::Mat color = colorImgs[i]; 
cv::Mat depth = depthImgs[i]; 
Eigen::Isometry3d T = poses[i]; 
for ( int v=0; v<color.rows; vt+ ) 
for ( int u=0; u<color.cols; u++ ) 
{ 
unsigned int d = depth.ptr«unsigned short? ( v )[u]; // 深度 值 
if ( d--0 ) continue; // 为 0 表示 没有 测量 到 


if ( d >= 7000 ) continue; // RRAKM ABE, KH 
Eigen::Vector3d point; 

point [2] double (d)/depthScale; 

point[0] = (u-cx)*point[2]/fx; 


point[1] = (v-cy)*point[2]/fy; 
Eigen::Vector3d pointWorld = T*point; 


PointT p ; 
p.x = pointWorld[0]; 
p.y = pointWorld[1]; 
p.z = pointWorld[2]; 
p.b = color.data[ v*color.steptu*color.channels() ]; 
p.g = color.data[ v*color.steptu*color.channels()+1 ]; 
p.r = color.data[ v*color.steptu*color.channels()+2 ]; 
current->points.push_back( p ); 
) 
// depth filter and statistical removal 
PointCloud::Ptr tmp ( new PointCloud ); 
pel: :Statistical0utlierRemoval<PointT> statistical filter; 
statistical filter.setMeanK(50); 
statistical filter.setStddevMulThresh(1.0); 
statistical filter.setInputCloud(current); 
statistical filter.filter( *tmp ); 
(*pointCloud) += *tmp; 


pointCloud-»is dense = false; 
cout««" 5 A J- $ " ««pointCloud-»size()««"^* X . "««endl; 


// vozel filter 

pcl::VoxelGrid<PointT> voxel filter; 

voxel filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution 
PointCloud::Ptr tmp ( new PointCloud ); 

voxel filter.setInputCloud( pointCloud ); 

voxel filter.filter( *tmp ); 

tmp->swap( *pointCloud ); 


cout<<"#RZG, RAHA"<<pointCloud->size()<<"4,% . "««end1; 


pcl::io::savePCDFileBinary("map.pcd", *pointCloud ); 


return O0; 


我 们 的 思路 没有 太 大 变化 ， 主 要 不 同 之 处 在 于 : 

1. 在 生成 每 帧 点 云 时 ， 去 挥 深度 值 太 大 或 无 效 的 点 。 这 主要 是 考 上 让 
到 Kinect 的 有 效 量程 ， 超 过 量程 乙 后 的 深 度 值 会 有 较 大 误 大 。 

2. 利 用 统计 滤波 髓 方法 去 除 抓 立 扣 。 该 滤波 蓓 统计 每 个 氮 与 距离 它 
最 近 的 六 个 扣 的 距离 值 的 分 布 ， 去 除 距 离 均 值 过 大 的 把 。 这 样 ， 我 们 你 
Fd J AREER TE, ARE SIAR A o 


3. 最 后 ， 利 用 体 素 渡 波 器 Voxel Filter) 进行 降 采样 。 由 于 多 个 视 
角 存 在 视野 重 琶 ， 在 重 全 区域 会 存在 大 量 的 位 置 十 分 相近 的 点 。 这 会 白 
白 占 用 许多 内 存 空间 。 体 素 滤 波 保证 了 在 某 个 一 定 大 小 的 立方 体 ( 或 称 
体 素 ) 内 仅 有 一 个 点 ， 相 当 于 对 三 维 空间 进行 了 降 采样 ， 从 而 可 节省 很 
多 存储 空间 。 


图 13-8 显 示 了 滤波 前 后 的 对 比 图 。 左 侧 是 第 5 讲 程序 生成 的 点 云 地 
图 ， 而 右 侧 征 经 过 滤 流 后 的 点 云 地 图 。 观 察 白 色 框 中 部 分 ， 可 以 看 到 在 
滤波 前 存在 着 由 噪声 产生 的 许多 孤立 的 点 。 经 过 统计 外 点 去 除 之 后 ， 我 
们 消去 了 这 些 噪声 ， 使 得 整个 地 图 变 得 更 干 汶 。 为 一 方面 ， 我 们 在 体系 
滤波 右 中 ， 把 分 辩 识 调 至 0.01， 表 示 每 立方 硅 米 有 一 个 后。 这 是 一 个 比 
KEDIK, AA Ae Pee ot AS SA Zee, PRT 
出 中 可 以 看 到 点 数 明 显 减 少 了 许多 《从 90 万 个 点 减 到 了 44 万 个 点 ， 去 除 
一半 左 右 ) 。 





滤波 前 滤波 后 
图 13-8 ”滤波 前 后 的 对 比 图 (你 可 能 需要 放大 才能 看 清 ， 或 者 日 己 在 电脑 上 尝试 一 下 ) o 


点 云 地 图 为 我 们 拓 贷 了 比较 基本 的 可 视 化 地 图 ， 让 我 们 能 够 大 致 了 
解 环境 的 梓 子 。 它 以 三 维 方式 存储， 使 得 我 们 能 够 快速 地 浏览 场景 的 各 








个 角落 ， 旋 至 在 场景 中 进行 瘟 洲 。 氮 云 的 一 大 优势 是 可 以 二 接 由 RGB-D 
图 像 高 效 地 生成 ， 不 需要 额外 处 理 。 它 的 滤波 操作 也 非常 直观 ， 且 处 理 
效率 尚 能 接受 。 

不 过 ， 使 用 扣 云 表达 地 图 仍然 是 十 分 初级 的 ， 我 们 不 妨 按 照 之 前 提 
的 对 地 图 的 需求 ， 看 看 点 云 地 图 是 含 能 满 丰 。 

1. 定 位 需求 : 取决 于 前 站 视觉 里 程 计 的 处 理 方式 。 如 条 是 基于 特征 
扩 的 视 沉 里 程 计 ， 由 于 扣 云 中 没有 和 存储 特征 点 信 息 ， 所 以 无 法 用 于 基于 
特征 点 有 的 定位 方法 。 如 果 前 问 是 点 云 的 CP， 那么 可 以 考虑 将 局 部 扣 云 
对 全 局 后 云 进 行 ICP 以 估计 位 姿 。 然 而 ， 这 要 求全 局 扣 云 具有 较 好 的 精 
度 。 在 我 们 这 种 处 理 点 云 的 方式 中 ， 并 没有 对 后 云 本 里 进行 优化 ， 所 以 
征 不 够 的 。 


2. 导 航 与 避 障 的 需求 : 无 法 二 搂 用 于 导航 和 如 障 。 纯 粹 的 氮 云 无 法 
表示 “古人 否 有 障 得 物 ” 的 信息 ， 我 们 也 无 法 在 点 云 中 做 “任意 空间 氮 是 合 
馈 占 据 ” 这 样 的 查询 ， 而 这 是 导航 和 避 障 的 基本 需要 。 不 过 ， 可 以 在 点 
云 基础 上 进行 加 工 ， 得 到 更 适合 导航 与 避 障 的 地 图 形式 。 


3. 可 视 化 和 交互 : 具有 基本 的 可 视 化 与 交互 能 力 。 我 们 能 够 看 a 到场 
景 的 外 观 ， 也 能 在 场景 里 漫游 。 从 可 视 化 角度 来 说 ， 由 于 点 云 只 含有 离 
散 的 点 ， 而 没有 物体 表面 信息 〈 例 如 法 线 ) ， 所 以 不 太 符 合 人 们 的 可 视 
化 习惯 。 例 如 ， 扣 云 地 图 的 物体 从 正面 看 和 背面 看 是 一 样 的， 而 且 还 能 
透 过 物体 看 到 它 至 后 的 东西 : 这 坚 都 不 符合 我 们 日 芝 的 经 验 ， 因 为 我 们 
没有 物体 表面 的 信息 。 


综 上 所 述 ， 我 们 说 点 云 地 图 是 “基础 的 ”或 “初级 的 ”， 是 指 它 更 接近 
于 传 感 如 读 取 的 原始 数据 。 它 具有 一 些 基 本 的 功能 ， 但 通常 用 于 调试 和 
基本 的 显示 ， 不 便 和 直接 用 于 应 用 程序 。 如 末 我 们 希望 地 图 有 更 局 级 的 功 
能 ， 点 云 地 图 是 一 个 不 错 的 出 发 点 。 例 如 ， 人 针对 导航 功能 ， 我 们 可 以 从 
点 云 出 发 ， 构 建 占据 网 格 地 图 (Occupancy Grid) ， 以 供 导 航 算法 查询 
See AAT HI. FRA, SIMA ASIA Re eae) 方法 ， 束 能 通过 
FLASH EA ZS EER AR A, FEBS TA a I. BRYA EP, 
Surfel 尔 是 一 种 表达 物体 表面 的 方式 ， 以 面 元 作为 地 图 的 基本 单位 ， 能 
够 建立 漂亮 的 可 视 化 地 图 71 。 


图 13-9 显 示 了 泪 松 重建 和 Surfel 的 一 个 样 例 ， 可 以 看 到 它们 的 视 贫 


效果 明显 优 于 纯粹 后 云 建 图 ， 而 它们 部 可 以 退 过 后 云 进行 构建 。 大 部 分 
由 点 云 转换 得 到 的 地 图 形式 都 在 PCL 库 中 提供 ， 感 兴趣 的 读者 可 以 进 一 
步 探 索 PCL 库 内 容 。 而 本 书 作为 入 门 材料 ， 束 不 评 尽 地 介绍 每 一 种 地 图 
形式 了 。 





泊 松 重建 示例 Surfel 重 建 示例 
图 13-9” 泊 松 重 建 与 Surfel 的 示意 图 


13.5.2” 八 义 树 地 图 


下 面 介 绍 一 种 在 寻 航 中 比较 章 用 的 ， 本 喘 有 较 好 的 压缩 性 能 的 地 笑 


在 点 云 地 图 中 ， 我 们 虽然 有 了 三 维 结构 ， 也 进行 了 体系 滤波 以 调整 
THES, {AERA LSA T EY aR Be 


.点 云 地 图 通常 规模 很 大 ， 所 以 pcd 文 件 也 会 很 大 。 一 幅 640 像 素 x 
480 像 素 的 图 像 ， 会 产生 30 万 个 空间 点 ， 需 要 大 量 的 存储 空间 。 即 使 经 
过 一 些 滤波 后 ，pcd 文 件 也 是 很 大 的 。 而 且 讨 厌 之 处 在 于 ， 它 的 “大 * 并 
不 是 必需 的 。 点 云 地 图 提供 了 很 多 不 必要 的 细节 。 对 于 地 毯 上 的 福 铁 、 
明暗 处 的 影子 ， 我 们 并 不 特别 关心 这 些 东西 。 把 它们 放 在 地 图 里 是 在 浪 
费 空间 。 由 于 这 些 空间 的 占用 ， 除 非 我 们 降低 分 辨 率 ， 否 则 在 有 限 的 内 
存 中 ， 无 法 建 模 较 大 的 环境 。 然 而 降低 分 辩 率 会 导致 地 图 质量 下 降 。 有 
没有 什么 方式 对 地 图 进行 压缩 存储 ， 合 弃 一 些 重复 的 信息 呢 ? 

“点 云 地 图 无 法 处 理 运动 物体 。 因 为 我 们 的 做 法 里 只 有 “添加 点 ”， 
而 没有 “ 当 点 消失 时 把 它 移 除 的 做 法 。 而 在 实际 环境 中 ， 运 动物 体 的 普 
遍 存 在 ， 使 得 点 云 地 图 变 得 不 够 实用 


MEM RESTA, me PPR A. FRAGA. SC REGEN SE it 
的 地 图 形式 : JAXA COcto-map) !!9 。 

我 们 知道 ， 把 三 维 空间 建 模 为 许多 个 小 方块 (或 体系 ) XÉ— nS N 
的 做 法 。 如 末 我 们 把 一 个 小 方块 的 每 个 面 平均 切 成 两 片 ， 那 么 这 个 小 方 
块 束 会 变 成 同样 大 小 的 八 个 小 方块 。 这 个 步 又 可 以 不 断 地 重复 ， 直 到 最 
后 的 方块 大 小 达到 建 模 的 最 高 精度 。 在 这 个 过 程 中 ， 把 “将 一 个 小 方块 
分 成 同样 大 小 的 八 个 ”这 件 事 ， 看 成 “从 一 个 节点 展开 成 八 个 子 太 点 ”， 
那么 ， 人 整个 从 最 大 衬 间 细 分 到 最 小 空间 的 过 程 ， 束 是 一 标 八 又 树 
(Octo-tree) 。 


如 图 13-10 所 示 ， 左 侧 显 示 了 一 个 大 立方 体 不 断 地 均匀 分 成 八 块 ， 
直到 变 成 最 小 的 方块 为 止 。 于 是 ， 整 个 大 方块 可 以 看 成 是 根 节点 ， 而 最 
小 的 块 可 以 看 作 是 “叶子 节点 ”。 于 是 ， 在 八 又 树 中 ， 当 我 们 由 下 一 层 节 
点 往 上 走 一 层 时 ， 地 图 的 体积 束 能 扩大 为 原来 的 八 倍 。 我 们 不 妨 做 一 点 
简单 的 计算 : 如 果 叶 子 节点 的 方块 大 小 为 1 cm ， 那 么 当 我 们 限制 八 又 
树 为 10 层 时 ， 总 共 能 建 模 的 体积 大 约 为 80 cm? =1, 073m? ， 这 足够 建 模 
一 间 屋 子 。 由 于 体积 与 深度 呈 指 数 关 系 ， 所 以 当 我 们 用 更 大 的 深度 时 ， 
建 模 的 体积 会 增长 得 非常 快 。 

读者 可 能 会 疑惑 ， 在 点 云 的 体 素 滤波 咒 中 ， 我 们 不 是 也 限制 了 一 个 
体 素 中 只 有 一 个 点 吗 ? 为 何 我 们 说 点 云 占 空间 ， 而 八 叉 树 比较 节省 空间 
We? 这 是 因为 ， 在 八 又 树 中 ， 我 们 在 节点 中 存储 它 是 否 被 占据 的 信息 。 
然而 ， 不 同 之 处 在 于 ， 当 某 个 方块 的 所 有 子 节 点 都 被 占据 或 都 不 被 占据 
时 ， 就 没 必要 展开 这 个 节点 “。 人 例如， 一 开始 地 图 为 空白 时 ， 我 们 惑 只 
需 一 个 根 节点 ， 而 不 需要 完整 的 树 。 当 癌 地 图 中 添加 信息 时 ， 由 于 实际 
的 物体 经 常 连 在 一 起 ， 衬 白 的 地 方 也 会 常常 连 在 一 起 ， 所 以 大 多 数 八 又 
树 节 点 都 无 顷 展 开 到 叶子 层面 。 所 以 说 ， 八 又 树 比 点 云 节 省 大 量 的 存储 


TH 


$ RTR 
TO OOOO a 


Ah 一 上 

AQOOQOOUUQO B 
` 
` 
` 


daw. 叶子 层 


图 13-10” 八 叉 树 示意 图 


前 面 说 八 叉 树 的 节 反 存储 了 它 是 侍 被 占据 的 信息 。 从 后 云层 面 来 
讨 ， 我 们 上 日 然 可 以 用 0 表示 空 日 ，1 表 示 被 占据 。 这 种 0-1 的 表示 可 以 用 
一 个 比特 来 存储 ， 市 省 空间 ， 不 过 显得 有 些 过 于 人 简单 了 。 由 于 噪声 的 影 
啊 ， 我 们 可 能 会 看 到 茶 个 点 一 会 儿 为 0， 一 会 儿 为 1 或 者 大 部 分 时 刻 为 
0， 小 部 分 时 刻 为 1 或 者 除 fe" a’ MZ, IBAA 
知 ” 的 状态 。 能 人 否 更 精细 地 描述 这 件 事 呢 ? 我 们 会 选择 用 概率 形式 表达 
A Hed uda. bu. H—TvURAGMxe [0，1] 来 表达 。 这 
个 x —JT38HX0.5.. SUR AS RME E THe, XA VEXXCAI ELA THES IM 
RZ, AB SES ARLE T ergo] BUR]. 


I A, RAISER SIA ta ek. GL. HO 
在 的 方式 有 一 点 小 问题 ， 如 来 让 x 不 断 增 加 或 减 小 ， 它 可 能 跑 到 [0, 11X 
则 之 外 ， 市 来 处 理 上 的 不 便 。 所 以 我 们 不 是 耳 接 用 概 京 来 插 述 作 市 点 被 
占据 ， 而 是 用 概率 对 数值 (Log-odds ) RHR. Wye R 为 概率 对 数 
值 ，x 为 0 一 1 的 概率 ， 那 么 它们 之 同 的 变换 由 logit 变 换 摘 述 : 








y = logit(x) = log (=) l (13,17) 
—2 


其 反 变 换 为 


exp(y) 
e) +1 (13.18) 


可 以 看 到 ， 当 y 从 -oo 变 到 +oo 时 ，x 相应 地 从 0 变 到 了 1。 而 当 y 取 0 
时 ，x 取 到 0.5。 因 此 ， 我 们 不 妨 存储 y 来 表达 节点 是 否 被 占据 。 当 不 断 
观测 到 “占据 ?时 ， 让 y 增加 一 个 值 :; 否则 惑 让 y 减 小 一 个 值 。 当 查询 概 
率 时 ， 再 用 逆 logit 变 换 ， 将 y 转换 至 概率 即 可 。 用 数学 形式 来 说 ， 设 某 
节点 为 n ， 观 测 数据 为 7 。 那 么 从 开始 到 # 时 刻 菜 节点 的 概率 对 数值 为 L 
(nz )，t+l 时 刻 为 


x —logit (y) = 


L(n|z1:t41) 一 L(n|2z1:—1) + L(n|zz). (13.19) 
BHR Sy ABESSE IZ SAI A ce BO, aS ALAR: 
—1 


] 一 P(n|zr) ] 一 P(n|zi.T—1) P (n) 
P(n|zT) P(n|zT-1) 1- P(n) 


有 了 对 数 概率 ， 我 们 就 可 以 根据 RGB-D 数 据 更 新 整个 八 义 树 地 图 
了 。 假 设 我 们 在 RGB-D 图 像 中 观测 到 条 个 像 系 市 有 深度 d ， 这 说 明了 一 
FE: 我 们 在 深度 值 对 应 的 空间 点 上 观 凤 到 了 一 个 占据 数据 ， 并 且 ， 
从 相机 交心 出 发 到 这 个 点 的 线段 上 ， 应 该 是 没有 物体 的 《人 否则 会 被 遮 
HD 。 利 用 这 个 信息 ， 可 以 很 好 地 对 八 又 树 地 图 进行 更 新 ， 并 且 能 处 理 
运动 的 结构 。 


13.5.3 ”实践 八 又 树 地 图 


下 面 通 过 程序 演示 一 下 octomap 的 建 图 过 程 。 首 先 ， 请 读者 安装 
octomap/#: https://github.com/OctoMap/octomap. Octomap/ + 2417 
octomap 地 图 与 octovis (一 个 可 视 化 程序 ) ， 二 者 都 是 cmake 工 程 。 其 主 
要 依赖 doxygen， 可 通过 如 下 命令 安装 : 


1 | sudo apt-get install doxygen 


ERAT EL Be RC n fe] 388 351: B TR] IS ESL HE COS AR, AJ 


(13.20) 





P(n|zi.7) = |1- 


它 男 出 来 。 
四 slambook/ch13/dense RGBD/octomap mapping.cpp CH EX) 


1 | *Xinclude <iostream> 
2 | include <fstream> 


3 |using namespace std; 


5 |#include <opencv2/core/core.hpp> 


6 | #include <opencv2/highgui/highgui .hpp> 
s | #include <octomap/octomap.h> // for octomap 


10 | #include <Eigen/Geometry> 
1 | #include <boost/format.hpp> // for formating strings 


3 | int main( int argc, char** argv ) 


| 4 
15 // MR Fe c EIR ICE n 
16 cout<<" 正 在 将 图 像 转换 为 Octomap ..."««endl; 


18 // octomap tree 


octomap::ÜcTree tree( 0.05 ); // KRHA x 


for ( int i=0; i45; i++ ) 


1 
cout««" $k 2& ARP: "<<it+1<<endl; 
cv::Mat color = colorImgs[i]; 
cv::Mat depth = depthImgs[i]; 
Eigen::Isometry3d T = poses [ij ; 
octomap::Pointcloud cloud; // the point cloud in octomap 
for ( int v=0; v«color.rows; v-** ) 
for ( int u=0; u<color.cols; u** ) 
{ 
unsigned int d = depth.ptr«unsigned short? ( v )[u]; // 深度 值 
if ( d==0 ) continue; // 为 0 表示 没有 测量 到 
if (d >= 7000 ) continue; // 深度 太 大 时 不 稳定 ， 去 掉 
Eigen::Vector3d point; 
point[2] = double(d)/depthScale; 
point[0] = (u-cx)*point [2] /fx; 
point[1] = (v-cy)*point[2]/fy; 
Eigen::Vector3d pointWorld = T*point; 
// 将 世界 坐标 系 的 点 放 入 点 云 
cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] ); 
) 
// 将 点 云 存 入 八 又 树 地 图 ， 给 定 原点 ， 这 样 可 以 计算 投射 线 
tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) ); 
) 


// 更 新 中 间 节 点 的 占据 信息 并 写 入 磁盘 
tree,updateInner0ccupancy() ; 
cout<<"saving octomap ... "««endl; 
tree.writeBinary( "octomap.bt" ); 


return 0; 


我 们 使 用 了 octomap::OcTree 来 构建 整 张 地 图 。 实 际 上 ， 


octomap 所 


供 了 许多 种 八 义 树 : 有 市 地 图 的 ， 有 市 占据 信息 的 ， 你 也 可 以 目 己 定义 
每 个 节点 需要 携 市 哪些 变量 。 价 单 起 见 ， 我 们 使 用 了 不 市 两 色 信息 的 、 
最 其 本 的 人 又 树 地 图 。 


Ocotmap 内 部 提供 了 一 个 点 云 结 构 。 它 比 PCL 的 点 云 稍 微 徐 单一 
些 ， 只 携 市 点 的 空间 位 置信 息 。 我 们 根据 RGB-D 疼 像 和 相机 位 姿 信息 ， 
先 将 点 的 坐标 转 全 世界 坐标 ， 然 后 放 入 octomap 的 点 云 ， 最 后 交 给 八 叉 
树 地 图 。 之 后 ，octomap 会 根据 之 前 介绍 的 投影 信息 ， 更 新 内 部 的 占据 
概 靳 ， 最 后 保存 成 压缩 后 的 八 叉 树 地 图 。 我 们 把 生成 的 地 图 存 成 
octomap.bt 文 件 。 在 之 前 编译 octovis 时 ， 我 们 实际 上 安 冯 了 一 个 可 视 化 
程序 ， 即 octovis。 现 在 ， 调 用 它 打 开 地 图 文件 ， 残 能 看 到 地 图 的 实际 样 
fle 


图 13-11 显 示 了 我 们 构建 的 地 图 结果 。 由 于 我 们 没有 在 地 图 中 加 入 
颜色 信息 ， 所 以 一 开始 打开 地 图 时 将 是 灰色 的 ， 按 “12 键 可 以 根据 高 虔 
信息 进行 染色 。 读 者 可 以 熟悉 一 下 octovis 的 操作 界面 ， 包 括 地 图 的 查 
看 、 旋 转 、 缩 放 等 。 














| Single node: 16 B; Octree: 4,151,744 B (3.959 MB) 








八 叉 树 地 图 (0.05 米 分 状 率 八 叉 树 地 图 (0.1 米 分 状 率 ) 
图 13-11 八 叉 树 地 图 在 不 同 分 辨 率 下 的 显示 结果 


在 右 侧 有 八 叉 树 的 深度 限制 条 ， 这 里 可 以 调节 地 图 的 分 辨 紊 。 由 于 
我 们 构造 时 使 用 的 默认 深度 是 16 屋 ， 所 以 这 里 显示 16 层 即 最 高 分 辨 率 ， 
也 就 是 每 个 小 块 的 边 长 为 0.05 米 。 当 我 们 将 深度 减少 一 层 时 ， 八 叉 树 的 
叶子 节点 往 上 提 一 层 ， 每 个 小 块 的 边 长 就 增加 一 倍 ， 变 成 0.1 米 。 可 以 
看 到 ， 我 们 能 够 很 容易 地 调节 地 图 分 辩 率 以 适应 不 同 的 场合 。 


Octomap 还 有 一 些 可 以 探索 的 地 方 ， 例 如 ， 我 们 可 以 方便 地 查询 任 
意 扣 的 占据 概 潜 ， 以 此 设计 在 地 图 中 进行 导航 的 方法 MH 。 读 者 亦 可 比 
较 点 云 地 图 与 八 驻 树 地 图 的 文件 大 小 。 上 一 和 生成 的 点 云 地 图 的 厂 盘 文 
件 大 约 为 6.9MB， 而 octomap 只 有 56KB， 连 点 云 地 图 的 百 分 之 一 都 不 
到 ， 可 以 有 效 地 建 模 较 大 的 场景 。 


13.6 *TSDF 地 图 和 Fusion 系 列 


在 本 讲 的 最 后 ， 我 们 介绍 一 个 与 SLAM 非 党 相似 但 义 有 稍 许 不 同 的 
WATA: 实时 三 维 重 建 。 本 节 内 容 牵 水 到 GPU 编程 ， 并 未 据 供 参考 例 
子 ， 所 以 作为 可 选 的 阅读 材料 。 


在 前 面 的 地 图 模型 中 ， 以 定位 为 主体 ”。 地 图 的 拼接 是 作为 后 续 加 
工 步 又 放 在 SLAM 框 积 中 的 。 这 种 框架 成 为 主流 的 原因 是 ， 定 位 算法 可 
以 满足 实时 性 的 需求 ， 而 地 图 的 加 工 可 以 在 关键 师 处 进行 处 理 ， 无 须 实 
时 啊 应 。 定 位 通 第 是 轻 量 级 的 ， 特 列 是 当 使 用 秘 焉 特征 或 牧 咏 百 技法 
时 ; 相应 地 ， 地 图 的 表达 与 存储 则 古 重 量 级 的 。 它 们 的 规模 和 计算 需求 
较 太 ， 不 利于 实时 处 理 。 特 别 是 稠密 地 图 ， 往 往 只 能 在 关键 师 层面 进行 
VR. 

但 是， 现 有 做 法 中 ， 我 们 并 没有 对 向 密 地 图 进行 优化 。 比 如 ， 妆 两 
昼 图 像 部 观察 到 同一 把 本子 时 ， 我 们 只 根据 两 幅 图 像 的 位 竣 把 两 处 的 后 
云 进行 登 如， 生成 了 地 图 。 由 于 位 姿 估计 通 帝 是 市 有 误 兰 的 ， 这 种 且 接 
拼接 往往 不 够 准确 ， 比 如 同一 把 椅子 的 点 云 无 法 完 类 地 登 加 在 一 起 。 这 
时 候 ， 地 图 中 会 出 现 这 把 椅子 的 两 个 重 影 一 这 种 现象 有 时 候补 形象 地 
MI HEE” o 


这 种 现象 显然 不 是 我 们 想 要 的 ， 我 们 布 望 重 建 结果 是 光 清 的 、 完 整 
的 ， 和 人 符合 我 们 对 地 图 的 认识 的 。 在 这 种 思想 下 ， 出 现 了 一 种 以 “ 建 图 ”为 
主体 ， 而 定位 抽 于 次 要 地 位 的 做 法 ， 也 就 是 本 节 要 介绍 的 实时 三 维 重 
建 。 由 于 三 维 重 建 把 重建 准确 地 图 作为 主要 目标 ， 所 以 基本 都 需要 利用 
GPU 进行 加 速 ， 其 至 需要 非 音 高 级 的 GPU 或 多 个 GPU 进行 并 行 加 速 ， 通 
弟 需 要 较 重 的 计算 设备 。 与 之 相反 ，SLAM 是 瑚 轻 量 级 、 小 型 化 方 同 友 
展 ， 有 些 方案 甚至 放弃 了 建 图 和 回环 检测 部 分 ， 只 保留 了 视 芝 里程 计 。 
而 实时 重建 则 正在 瑚 大 规模 、 大 型 动态 场景 的 备 建 方 同 发 展 。 

目 从 RGB-D 传 感 硕 出 现 以 来 ， 利 用 RGB-D 图 像 进行 实时 重建 形成 
了 一 个 重要 的 发 展 方向 ， 陆 续 产 生 了 Kinect Fusion ^ , Dynamic 
Fusion?" 、Elastic Fusion"?! , Fusion4DU?! , Volumn Deform'?^ 等 成 


果 。 其 中 ，Kinect Fusion 完 成 了 基本 的 模型 重建 ， 但 仅 限 于 小 型 场景 ; 





后 续 的 工作 则 是 将 它 同 大 型 的 、 运 动 的 ， 甚 至 变形 场景 下 拓展 。 我 们 把 
它们 看 成 实时 重建 一 个 大 类 的 工作 ， 但 由 于 种 英 楷 多 ， 不 可 能 详细 讨论 
每 一 种 的 工作 诛 理 。 图 13-12 展 示 了 一 部 分 重建 结 素 ， 可 以 看 到 这 些 建 
异 结 未 非常 精细 ， 比 单纯 拼接 点 云 要 细 肛 很 多 。 


我 们 就 以 经 典 的 TSDF 地 图 为 代表 进行 介绍 。TSDF 是 Truncated 
Signed Distance Function 的 缩写 ， 不 芒 详 作 帘 断 符 吕 距 离 国 数 。 虽 然 
E PRB PR YHA EAA BY, PATTER A BUPA ZA, RA] 
xe BIN PE ATSDFIEA. TSDFE ES, RE NEEN EZ B 
Ay. 


与 八 又 树 相 似 ，TSDF 地 图 也 是 一 种 网 格式 (或 者 说 ， 方 块 式 ) 的 
地 图 ， 如 图 13-13 所 示 。 先 选 定 要 建 模 的 三 维 空间 ， 比 如 3x 3x 3m? 那么 
大 ， 按 照 一 定 分 辩 率 将 这 个 空间 分 成 许多 小 块 ， 存 储 每 个 小 块 内 部 的 信 
轧 。 不 同 的 是 ，TSDEF 地 图 整个 存储 在 显存 而 不 是 内 存 中 。 利 用 GPU 的 
并 行 特性 ， 我 们 可 以 并 行 地 对 每 个 体系 进行 计算 和 更 新 ， 而 不 像 CPU 表 
历 内 存 区 域 那 样 不 得 不 串 行 。 


每 个 TSDF 体 素 内 ， 存 储 了 该 小 块 与 距 其 最 近 的 物体 表面 的 距离 。 
如 果 小 块 在 该 物体 表面 的 前 方 ， 它 束 有 一 个 正 的 值 ， 反 之 ， 如 果 该 小 块 
位 于 表面 之 后 ， 那 么 就 为 负 值 。 由 于 物体 表面 通常 是 很 注 的 一 层 ， 所 以 
就 把 值 太 大 的 和 太 小 的 都 取 成 1 和 - 1， 这 样 就 得 到 了 截断 之 后 的 距离 ， 
也 残 是 所 谓 的 TSDF。 那 么 按照 定义 ，TSDF 为 0 的 地 方丈 是 表面 本 喘 
或 者 ， 由 于 数值 误差 的 存在 ，TSDF 由 负 号 变 成 正 号 的 地 方 就 是 表 
面 本 喘 。 在 多 13-13 的 下 部 ， 我 们 看 到 一 个 类 似 于 人 脸 的 表面 出 现在 了 
TSDF 改 变 从 号 的 地 方 。 








a (b) i (c) 


N 





图 13-12 ”各 种 实时 三 维 重 建 的 模型 。 (a) Kinect Fusion; (b) Dynamic Fusion; (c) 
Volumn Deform; (d) FusiondD; (e) Elastic Fusion. 
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相机 观察 到 物体 表面 时 形成 的 截断 距离 值 
图 13-13 ”TSDF 示 意图 。 


TSDF 评 有 “定位 ”与 “ 建 图 ”两 个 问题 ， 与 SLAM 非 常 相似 ， 不 过 具体 
的 形式 与 本 书 前 面 几 讲 介绍 的 棚 有 不 同 。 在 这 里 ， 定 位 问题 主要 指 如 何 
把 当前 的 RGB-D 图 像 与 GPU 中 的 TSDF 地 图 进行 比较 ， 估 计 相 机 位 姿 。 
而 建 图 问题 就 古 如 何 根据 估计 的 相机 位 姿 ， 对 TSDF 地 图 进行 更 新 。 传 
统 做 法 中 ， 我 们 还 会 对 RGB-D 图 像 进行 一 次 双边 贝 叶 斯 滤波 ， 以 去 除 深 
FE HABE ES, 

TSDF 的 定位 类 似 于 前 面 介绍 的 ICP， 不 过 由 于 GPU 的 并 行 化 ， 我 们 
可 以 对 整 张 深度 图 和 TSDF 地 图 进行 ICP 计 算 ， 而 不 必 像 传统 视觉 里 程 计 


那样 必须 匈 计 算 特 征 点 。 同 时 ， 由 于 TSDF 没 有 颜色 信息 ， 意 味 看 我 们 
可 以 只 使 用 深度 图 ， 不 使 用 彩色 图 ， 束 完成 位 姿 佑 计 ， 这 在 一 定 程 度 
上 摆脱 了 视 视 4 里 程 计算 法 对 光照 和 纹理 的 依赖 性 ， 便 得 RGB-D 重 建 更 加 
稳健 申 ”。 帮 一 方面 ， 建 图 部 分 亦 是 一 种 并 行 地 对 TSDF 中 的 数值 进行 更 
新 的 过 程 ， 使 得 所 估计 的 表面 更 加 平滑 可 笔 。 由 于 我 们 并 不 过 多 介绍 
GPU 相 关 的 内 容 ， 所 以 具体 的 方法 就 不 展开 细 说 了 ， 请 感 感 兴趣 的 读者 参 
向 相关 文献 。 


13.7 小 结 


本 讲 介 绍 了 一 些 常见 的 地 图 类 型 ， 尤 其 是 稠密 地 图 形式 。 我 们 看 到 
根据 单 目 或 双 目 可 以 构建 稠密 地 图 ， 而 RGB-D 地 图 则 往往 更 加 容易 、 稳 
定 一 些 。 本 讲 的 地 图 偏重 于 度量 地 图 ， 而 拓扑 地 图 形式 由 于 和 SLAM 研 
究 差 别 比较 大 ， 所 以 没有 详细 地 展开 探讨 。 

习题 

1. 推 姓 式 (13.6)。 

2. 把 本 讲 的 稠密 深度 估计 改 成 半 笛 密 ， 你 可 以 先 把 梯度 明显 的 地 方 
fli 3e HO e 

3.38 ANH Ya Is HJ FEL HA a EE TLLA ET E CR E. JEAN 
仿 射 变换 。 你 的 实验 效果 是 否 有 改进 ? 

4. 你 能 论证 如 何在 八 又 树 中 进行 导航 或 路 径 规 划 吗 ? 

5. 研 究 文 献 [120]， 探 讨 TSDF 地 图 是 如 何 进行 位 姿 估 计 和 更 新 的 。 
它 和 我 们 之 前 讲 过 的 定位 建 图 算法 有 何 异 同 ? 

6.* 研 完 均 匀 - 高 斯 混合 滤波 器 的 原理 与 实现 。 





[1] 正式 点 叫 Fragile。 

[2] 反之 ， 如 果 运 动 不 知道 ， 那 么 极 线 也 无 法 确定 。 

[3] 整体 亮 一 些 可 能 由 环境 光照 变 亮 或 相机 曝光 参数 升 高 导致 。 

[4] 请 注意 ， 稠 密 深 度 估计 运行 比较 费时 ， 如 果 你 的 计算 机 比较 老 ， 请 耐心 等 修一 段 时 间 。 
[5] u,v 的 不 确定 性 取决 于 图 像 的 分 辨 率 。 

[6] 不 过 话说 回来 ， 对 深度 图 就 更 加 依赖 了 。 


第 14 讲 SLAM: 现在 与 未 来 


主要 目标 

1. 了 解 经 典 的 SLAM 实 现 方 案 。 

2. 退 过 实验 ， 比 较 各 种 SLAM 方 案 的 异同 。 

3. 探 讨 SLAM 的 未 来 发 展 方 问 。 

剖面 介绍 了 一 个 SLAM 系 统 中 的 各 个 模块 的 工作 原理 ， 这 是 研究 者 
们 多 年 工作 的 结晶 。 目 前 ， 除 了 这 些 理论 框架 之 外 ， 我 们 也 积累 了 许多 
优秀 的 开源 SLAM 方 案 。 不 过 ， 由 于 它们 大 部 分 的 实现 都 比较 复杂 ， 不 
适合 作为 初学 者 的 上 手 材料 ， 所 以 我 们 放 到 了 本 书 的 最 后 加 以 介绍 。 相 
信 读 者 通过 阅读 之 前 的 内 容 ， 应 该 能 明白 其 基本 原理 。 


14.1 当前 的 开源 方案 


本 讲 是 全 书 的 总 结 ， 我 们 将 带 着 读者 去 看 看 现 有 的 SLAM 方 案 能 做 
到 怎样 的 程度 。 特 别 地 ， 我 们 重点 关注 那些 提供 开源 实现 的 方案 。 在 
SLAM 人 研究 领域 ， 能 见 到 开源 方案 是 很 不 容易 的 。 往 往 论 文中 介绍 理论 
只 占 20% 的 内 容 ， 其 他 809% 都 写 在 代码 中 ， 是 论文 里 没有 提 到 的 。 正 是 
这 些 研究 者 们 的 无 私 奉献 ， 推 动 了 整个 SLAM 行 业 的 快速 前 进 ， 使 后 续 
研究 者 有 了 更 高 的 起 点 。 在 我 们 开始 做 SLAM 之 前 ， 应 该 对 相似 的 方案 
有 深入 的 了 解 ， 然 后 再 进行 自己 的 研究 ， 这 样 才 会 更 有 意义 。 

本 讲 的 前 半 部 分 将 带领 读者 参观 一 下 当前 的 视觉 SLAM 方 案 ， 评 述 
其 历史 地 位 和 优 缺 点 。 示 14-1 列 举 了 一 些 币 见 的 开 谣 SLAM 方 委 ， 旋 者 
可 以 选择 感 兴趣 的 方案 进行 研究 和 实验 。 限 于 篇 幅 ， 我 们 只 选 了 一 部 分 
有 代表 性 的 方案 ， 这 肯定 是 不 全 面 的 。 在 后 半 部 分 ， 我 们 将 探讨 未 来 可 
能 的 一 些 发 展 方向 ， 并 给 出 当前 的 一 些 研 究 成 果 。 


表 14-1 常 用 开源 SLAM 方 案 


方案 名 称 传感器 形式 | 地 址 


MonoSLAM 单 日 https://github.com/hanmekim/SceneLib2 
PTAM 单 日 http://www.robots.ox.ac.uk/-gk/PTAM/ 
ORB-SLAM 单 目 为 主 | http://webdiis.unizar.es/-raulmur/orbslam/ 
LSD-SLAM 单 目 为 主 | http://vision.in.tum.de/research/vslam/ 
lsdslam 
SVO 单 目 https://github.com/uzh-rpg/rpg svo 
DTAM RGB-D https://github.com/anuranbaka/ÜpenDTAM 
DVO RGB-D https://github.com/tum-vision/dvo slam 
DSO 单 日 https://github.com/JakobEngel/dso 


RTAB-MAP XXL H/RGB-D | https://github.com/introlab/rtabmap 
RGBD-SLAM-V2 RGB-D https://github.com/felixendres/rgbdslam v2 


Elastic Fusion RGB-D https://github.com/mp3guy/ElasticFusion 
Hector SLAM WO http://wiki.ros.org/hector_slam 
GMapping BO http://wiki.ros.org/gmapping 
OKVIS ZH +IMU | https://github.com/ethz-asl/okvis 





ROVIO #H +IMU | https://github.com/ethz-asl/rovio 


14.1.1 MonoSLAM 


说 到 视觉 SLAM， 很 多 研究 者 第 一 个 想到 的 是 A.J.Davison 的 曲目 
SLAM LÍE?5! 。Davison 教 授 是 视 训 SLAM 研 究 领 域 的 先驱 ， 他 在 2007 
年 提出 的 MonoSLAM 是 第 一 个 实时 的 蛙 目 视觉 SLAM 系 统 中 ， 被 认为 是 
许多 工作 的 发 源 地 由 MonoSLAMUAS Re FAR BYE Ze — XE E Bl rtg 
非常 稀 臣 的 特征 点 。 由 于 EKF 在 早期 SLAM 中 占据 着 明显 主导 地 位 ， 所 
以 MonoSLAM 杰 是 建立 在 EKF 的 基础 之 上 ， 以 相机 的 当前 状态 和 所 有 路 
DRAKA EH, SPA AMAT. 


14-1 aN zeMonoSLAMZ£ie (TII WT. Pa, $ A PALE 
—Wà VAS ides I JETÉ AE CALA BS ESI ERIN) 。 在 
EKF'B, [RAE SPP AR Me iat, Ar PA BRANT RE DA “a ERE 
形式 表达 它 的 均值 和 不 确定 性 。 在 该 图 的 右 半 部 分 ， 我 们 可 以 找到 一 些 
在 空间 中 分 布 看 的 小 球 。 它 们 在 条 个 方 同 上 喧 得 越 长 ， 说 明 在 该 方 同 的 
位 置 融 越 不 确定 。 我 们 可 以 想象 ， 如 果 一 个 特征 氮 收 伍 ， 我 们 应 该 能 


到 它 从 一 个 很 长 的 椭 球 相机 Z 方 同 上 非常 不 确定 〉 最 后 变 成 一 个 小 后 
的 样子 。 
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图 14-1 MonoSLAM 的 运行 时 截图 。 左 侧 : 追踪 特征 点 在 图 像 中 的 表示 ; dud: 特征 点 
在 三 维 空间 中 的 表示 。 


这 种 做 法 在 今天 看 来 固然 存在 许多 竞 问 ， 但 在 当时 已 经 是 里 程 夏 式 
的 工作 了 ， 因 为 在 此 之 前 的 视 训 SLAM 系 统 基本 不 有 在 线 运 介 ， H Bes 
Blas ATE WAAL KAA, FS zat Tea. TALE RE ie 
Dy VARA PN Jy hE AR, ME sae ches lane iy 
在 线 地 运行 。 从 现代 的 角度 来 看 ，MonoSLAM 存 在 诸如 应 用 场景 很 罕 ， 
路 标 数量 有 限 ， 稀 芷 特征 点 非 间 容易 丢失 的 情况 ， 对 它 的 开发 也 已 经 停 
止 ， 取 而 代 之 的 是 更 匈 进 的 理论 和 编程 工具 。 不 过 这 并 不 妨碍 我 们 对 前 
人 工作 的 理解 和 尊敬 。 


EX 








14.1.2 PITAM 


2007 年 ，Klein 等 人 捉 出 了 PTAM (Parallel Tracking and 


Mapping) 9! ， 这 也 是 视 党 SLAM 及 展 过 程 中 的 重要 事件 。PTAM 的 重 
要 意义 在 于 以 下 两 点 : 


1.PTAM 提 出 并 实现 了 跟踪 与 建 图 过 程 的 并 行 化 。 我 们 现在 已 然 清 
楚 ， 跟 踪 部 分 需要 实时 响应 图 像 数 据 ， 而 对 地 图 的 优化 则 没 必要 实时 地 
计算 。 后 端 优化 可 以 在 后 台 慢 慢 进行 ， 然 后 在 必要 的 时 候 进行 线程 同步 


即 可 。 这 是 视觉 SLAM 中 首次 区 分 出 前 后 端的 概念 ， 引 领 了 后 来 许多 视 
觉 SLAM 系 统 的 设计 《我 们 现在 看 到 的 SLAM 多 半 都 分 前 后 病 ) o 

2.PTAM 是 第 一 个 使 用 非 线 性 优化 ， 而 不 是 使 用 传统 的 滤波 器 作为 
后 端的 方案 。 它 引入 了 关键 帧 机 制 : 我 们 不 必 精 细 地 处 理 每 一 幅 图 像 ， 
而 是 把 几 个 关键 图 像 串 起 来 ， 然 后 优化 其 轨迹 和 地 图 。 早 期 的 SLAM 大 
多 数 使 用 EKF 滤 波 右 或 其 变种 ， 以 及 粒子 滤波 右 等 ， 在 PTAM 之 后 ， 视 
训 SLAM 研 究 逐 渐 转 同 了 以 非 线 性 优化 为 主导 的 后 端 。 由 于 之 前 人 们 未 
认识 到 后 闯 优 化 的 稀 杞 性 ， 所 以 和 觉得 优化 后 内 无 法 实时 处 理 那 样 大 规 横 
的 数据 ， 而 PTAM 则 是 一 个 显著 的 反例 。 

PTAM 同 时 是 一 个 增强 现实 软件 ， 泪 示 了 酷 炫 的 AR 效果 〈 如 图 14-2 
所 示 ) 。 根 据 PTAM 估 计 的 相机 位 姿 ， 我 们 可 以 在 一 个 虚拟 的 平面 上 放 
置 虚 拟 物 体 ， 看 起 来 殉 像 在 真实 的 场景 中 一 样 。 


mex EDS 
dr à 








图 14-2 PTAMIRBJIZWEXE. BER] EESK EMER, qn] De eA Len 


虚拟 物体 。 


不 过 ， 从 现代 的 眼光 看 来 ，PTAM 也 算是 早期 的 结合 AR 的 SLAM 工 
作 之 一 。 与 许多 早期 工作 相似 ， 存 在 看 明显 的 缺陷 :场景 小 ， 跟 踪 容 吻 
丢失， 等 等 。 这 些 义 在 后 续 的 方 条 中 得 以 修正 。 


14.1.3 ORB-SLAM 


介绍 了 历史 上 的 几 种 方案 之 后 ， 我 们 来 看 现代 的 一 些 SLAM 系 统 。 
ORB-SLAM 是 PTAM 的 继承 者 中 非 营 有 名 的 一 位 5C3 〈 见 图 14-3) . “Ete 
出 于 2015 和 年， 是 现代 SLAM 系 统 中 做 得 非常 完善 、 非 音 易 用 的 系统 之 一 
(如 果 不 是 最 完善 易 用 的 话 ) 。ORB-SLAM 代 表 着 主流 的 特征 点 SLAM 


的 一 个 高 峰 。 相 比 于 之 前 的 工作 ，ORB-SLAM 具 有 以 下 几 条 明显 的 优 
势 : 


1. 文 持 单 目 、 双 目 、RGB-D 三 种 模式 。 这 使 得 无 论 我 们 拿 到 了 哪 种 
常见 的 传感器 ， 都 可 以 先 放 到 ORB-SLAM 上 测试 一 下 ， 它 具有 良好 的 泛 
FA TH: « 


2. 整 个 系统 围 经 ORB 特 征 进 行 计 算 ， 包 括 视 和 沉 里 程 计 与 回环 检测 的 
ORB 字 典 。 它 体现 出 ORB 特 征 是 现 阶段 计算 平台 的 一 种 优秀 的 效率 与 精 
度 之 间 的 折 中 方式 。ORB 不 像 SIFT 或 SURE 那 样 费 时 ， 在 CPU 上 面 即 可 
实时 计算 ; 相 比 Harris 角 点 等 答 单 角 点 特征 ， 又 具有 民 好 的 旋转 和 缩放 
不 变性 。 并 且 ，ORB 提 供 摘 述 子 ， 使 我 们 在 大 范围 运动 时 能 够 进行 回环 
检测 和 重 定位 。 

3.ORB 的 回环 检测 是 它 的 亮点 。 优 秀 的 回环 检测 算法 保证 了 ORB- 
SLAM 有 效 地 防止 票 积 误 兰 ， 并 且 在 丢失 之 后 还 能 迅速 找 回 ， 这 一 点 许 
多 现 有 的 SLAM 系 统 都 不 够 完善 。 为 此 ，ORB-SLAM 在 运行 之 前 必须 加 
载 一 个 很 大 的 ORB 字 — 典 文件 中。 


4.0RB-SLAM 创 新 式 地 使 用 了 三 个 线程 完成 SLAM: 实时 跟踪 特征 
点 的 Tracking 线 程 ， 局 部 Bundle ”Adjustment 的 优化 线程 (Co-visibility 
Graph， 俗 称 小 图 。 ) ， 以 及 全 局 Pose Graph 的 回环 检测 与 优化 线程 
(Essential Graph 俗称 大 图 ) 。 其 中 ，Tracking 线 程 负 责 对 每 幅 新 来 的 
图 像 提 取 ORB 特 征 点 ， 并 与 最 近 的 关键 巾 进 行 比较 ， 计 算 特 征 点 的 位 置 


并 粗略 估计 相机 位 姿 。 小 图 线程 求解 一 个 Bundle Adjustment) wi, Ee, 
括 局 部 空间 内 的 特征 点 与 相机 位 姿 。 这 个 线程 负责 求解 更 精细 的 相机 位 
次 与 特征 点 空间 位 置 。 不 过 ， 仪 有 前 两 个 线程 ， 只 完成 了 一 个 比较 好 的 
人 钢 觉 里程 计 。 第 三 个 线程 ， 也 就 是 大 图 线程 ， 对 全 局 的 地 图 与 关键 帧 进 
行 回环 检测 ， 消 除 系 积 误 去 。 由 于 全 局 地 图 中 的 地 图 点 太 多 ， 所 以 这 个 
线程 的 优化 不 包括 地 图 点 ， 而 只 有 相机 位 姿 组 成 的 位 姿 图 。 

继 PTAM 的 双 线 程 结构 之 后 ，ORB-SLAM 的 三 线程 结构 取得 了 非常 
好 的 跟踪 和 建 图 效果 ， 能 够 保证 轨迹 与 地 图 的 全 局 一 致 性 。 这 种 三 线程 
结构 也 将 被 后 续 的 研究 者 认同 和 采用 。 
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114-3. ORB-SLAM 运 行 截图 。 左 侧 为 图 像 与 追踪 到 的 特征 点 ， 右 侧 为 相机 轨迹 与 建 模 
的 特征 点 地 图 。 下 方 为 其 标志 性 的 三 线程 结构 。 


5.ORB-SLAM 围 经 特征 点 进行 了 不 少 的 优化 。 人 例如， 在 OpenCV 的 
特征 提取 基础 上 保证 了 特征 点 的 均匀 分 布 ， 在 优化 位 姿 时 使 用 了 一 种 循 
环 优 化 4 所 以 得 到 更 多 正确 匹配 的 方法 ， 比 PTAM 更 为 宽松 的 关键 帧 选 
取 策 略 ， 等 等 。 这 些 细小 的 改进 使 得 ORB-SLAM 具 有 远 超 其 他 方案 的 稳 
健 性 ， 即 使 对 于 较 差 的 场景 ， 较 差 的 标定 内 参 ，ORB-SLAM 都 能 够 顺利 


Jh LE. 


上 述 这 些 优势 使 得 ORB-SLAM 在 特征 点 SLAM 中 达到 顶峰 ， 许 多 研 
究 工 作 都 以 ORB-SLAM 作 为 标准 ， 或 者 在 它 的 基础 上 进行 后 续 的 开发 。 
它 的 代码 以 清晰 易 读 彰 称 ， 有 者 完善 的 注释 ， 可 供 后 来 的 研究 者 进一步 
FERE 


当然 ，ORB-SLAM 也 存在 一 些 不 足 之 处 。 首 先 ， 由 于 整个 SLAM 系 
统 都 采用 特征 点 进行 计算 ， 我 们 必须 对 每 幅 图 像 都 计算 一 遍 ORB 特 征 ， 
这 是 非常 耗 时 的 。ORB-SLAM 的 三 线程 结构 也 给 CPU 带 来 了 较 重 的 负 
担 ， 使 得 它 只 有 在 当前 PC 架构 的 CPU 上 才能 实时 运算 ,移植 到 组 入 式 设 
备 上 则 有 一 定 困 难 。 其 次 ，ORB-SLAM 的 建 图 为 稀 琉 特征 点 ， 目 前 还 没 
有 开放 存储 和 读 取 地 图 后 重新 定位 的 功能 (虽然 从 实现 上 来 讲 并 不 困 
ME) 。 根 据 我 们 在 建 图 部 分 的 分 析 ， 黎 瑰 符 征 点 地 图 只 能 满 中 我 们 对 定 
位 的 需求 ， 而 无 法 提供 导航 、 避 障 、 交 互 等 诸多 功能 。 然 和 而， 如果 我 们 
仅 用 ORB-SLAM 处 理 定 位 问题 ， 似 乎 又 显得 有 些 过 于 重量 级 了 。 相 比 之 
下 ， 另 外 一 些 方案 提供 了 更 为 轻 量 级 的 定位 ， 使 我 们 能 够 在 低 端 的 处 理 
器 上 运行 SLAM， 或 者 让 CPU 有 余力 处 理 其 他 的 事务 。 


14.1.4 LSD-SLAM 


LSD-SLAM (Large Scale Direct monocular SLAM) 是 JEngle 等 人 于 
2014F tE E ISLAM T6595 。 类 比 于 ORB-SLAM 之 于 特征 点 ，LSD- 
SLAM 则 标志 着 单 目 直接 法 在 SLAM 中 的 成 功 应 用 。LSD-SLAM 的 核心 
页 献 是 将 直接 法 应 用 到 了 半 移 密 的 单 目 SLAM 中 。 它 不 仅 不 需要 计算 特 
征 点 ， 还 能 构建 半 稠 密 的 地 图 一 一 这 里 半 稠 密 的 意思 主要 是 指 估计 梯度 
明显 的 像素 位 置 。 它 的 主要 优点 如 下 : 

1.LSD-SLAM 的 直接 法 是 针对 像素 进行 的 。 作 者 有 创见 地 所 出 了 像 
ZRBBEEÓE EESHEABIXA. URBE: n Ed zs ER HR 48 RE 
关系 。 这 些 在 本 书 的 第 8 讲 和 第 13 讲 均 有 讨论 。 不 过 ，LSD-SLAM 是 在 
单 目 图 像 进 行 半 稠密 的 跟踪 ， 实 现 原理 要 比 本 书 的 例 程 更 加 复杂 。 

2.LSD-SLAM 在 CPU 上 实现 了 半 笛 密 场 景 的 重建 ， 这 在 之 前 的 方 守 
中 是 很 少见 到 的 。 基 于 特征 点 的 方法 只 能 是 稀疏 的 ， 而 进行 稠密 重建 的 
方案 大 多 要 使 用 RGBD 传 感 希 ， 或 者 使 用 GPU 构建 稠密 地 图 上 0 , TUM 


计算 机 视 党 组 在 多 年 对 直接 法 研究 的 基础 上 上， 实现 了 这 种 CPU 上 的 实时 
PERS SLAM. 


3.2 Ath piii, LSD-SLAMPY = JR 2E XB Ex 8 H T. — Ee dS VP) HP BOR 
保证 追踪 的 实时 性 与 稳定 性 。 例 如 ，LSD-SLAM 既 不 是 利用 单个 像素 ， 
也 不 是 利用 图 像 块 ， 而 是 在 极 线 上 等 距离 取 5 个 点 ， 上 度量 其 SSD; Æ 
度 估 计时 ，LSD-SLAM 首 先 用 随机 数 初 始 化 深度 ， 在 佑 计 完 后 又 把 深度 
均值 归 一 化 ， 以 调整 尺度 ; 在 度量 深度 不 确定 性 时 ， 不 仅 考虑 三 角 化 的 
几何 关系 ， 而 且 考 虑 了 极 线 与 深度 的 夹 角 ， 归 纳 成 一 个 光度 不 确定 性 
项 ， 关 键 帆 之 则 的 约束 使 用 了 相似 变换 群 及 与 之 对 应 的 李 代 数 CE 
sim(3) 显 式 地 表达 出 尺度 ， 在 后 端 优化 中 可 以 将 不 同 尺 度 的 场景 考虑 进 
K, WDT REDENE. 
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密 地 图 是 怎样 一 种 介 于 稀 朴 地 图 与 稠密 地 图 之 间 的 形式 。 半 稠密 地 图 建 
模 了 灰 度 图 中 有 明显 梯度 的 部 分 ， 显 示 在 地 图 中 ， 很 大 一 部 分 都 是 物体 
的 边缘 或 表面 上 带 纹 理 的 部 分 。LSD-SLAM 对 它们 进行 跟踪 并 建立 关键 
帧 ， 最 后 优化 得 到 这 样 的 地 图 。 看 起 来 比 稀 臣 的 地 图 具有 更 多 的 信息 ， 
但 又 不 像 稠密 地 图 那样 拥有 完整 的 表面 〈 笛 密 地 图 一 般 认 为 无 法 仅 用 
CPU 实现 实时 性 ) 。 





图 14-4 LSD-SLAM 运 行 图 片 。 上 半 部 分 为 估计 的 轨迹 与 地 图 ， 下 半 部 分 为 图 像 中 被 建 模 
的 部 分 ， 即 具有 较 好 的 像素 梯度 的 部 分 。 


由 于 LSD-SLAM 使 用 了 生 接 法 进行 跟踪 ， 所 以 它 既 有 生 接 法 的 优 操 
(对 特征 缺失 区 域 不 敏感 ) ， 也 继承 了 年 接 法 的 缺 上 后。 例如 ，LSD- 
SLAM 对 相机 内 参 和 曝光 非 营 敏感 ， 并 且 在 相机 快速 运动 时 容易 丢 矢 。 
万 外 ， 在 回环 检 调 部分， 由 于 目前 并 没有 基于 直接 法 实现 的 回环 检 训 方 
却 ， 因 此 LSD-SLAM 必 须 依赖 于 特征 氮 方法 进行 回环 检测 ， 疝 未 完全 摊 
脱 特 征 点 的 计算 。 





14.15 SVO 


SVO 是 Semi-direct Visual Odoemtry 的 缩写 5 。 它 是 由 Forster 等 人 于 
2014 年 提出 的 一 种 基于 黎 焉 直接 法 的 视觉 里 程 计 。 按 作者 的 称呼 应 
hd“ 半 直接 ”法 ， 然 而 按照 本 书 的 理念 框架 ， 称 为 “ 稀 跑 直接 法 ”可 能 更 好 
一 些 。 半 有 直接” 在 原文 中 的 意思 是 指 特征 点 与 直接 法 的 混合 使 用 : SVO 
ERES S ERER AR KAMRET) ， 然 后 像 直 接 法 那样 ， 根 据 这 
些 关 键 点 周围 的 信息 估计 相 机 运动 及 其 位 置 “ 如 网 14-4 所 示 ) 。 在 实现 


中 ，SVO 使 用 了 关键 点 周围 的 4x 4 的 小 块 进行 块 匹配 ， 估 计 相 机 自身 的 
运动 。 

相 比 于 其 他 方案 ，SVO 的 最 大 优势 是 速度 极 快 。 由 于 使 用 稀 朴 的 直 
接 法 ， 它 既 不 必 费 力 去 计算 描述 子 ， 也 不 必 处 理 像 稠密 和 半 笛 密 那 么 多 
的 信息 ， 因 些 ， 即 使 在 低 端 计算 平台 上 也 能 达到 实时 性 ， 而 在 PC 平台 
上 则 可 以 达到 每 秒 100 多 帧 的 速度 。 在 后 续 的 SVO 2.0 中 ， 速 度 更 达到 了 
惊人 的 每 秒 400 帧 。 这 使 得 SVO 非 销 适 用 于 计算 平台 受 限 的 场合 ， 例 如 
无 人 机 、 手 持 AR/VR 设 备 的 定位 。 无 人 机 也 是 作者 开发 SVO 的 目标 应 用 
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图 14-5 ”SVO 跟 踪 关 键 点 的 图 片 。 


SVO 的 为 一 创 靳 之 处 是 提出 了 深度 滤波 帮 的 概 仿 ， 并 推 寻 了 基于 均 
匀 - 融 期 混合 分 布 的 深度 滤波 左 。 这 在 本 书 的 第 13 讲 有 所 及 ， 但 由 于 原 
理 较为 复杂 ， 我 们 没有 详细 解释 。SVO 将 这 种 滤波 器 用 于 关键 点 的 位 置 
估计 ， 并 使 用 了 逆 深 度 作为 参数 化 形式 ， 使 之 能 够 更 好 地 计算 特征 点 位 
置 。 


开源 版 的 SVO 代 码 清 晰 易 恋 ， 十 分 适合 读者 作为 第 一 个 SLAM 实 例 
进行 分 析 。 不 过 ， 开 源 版 SVO 也 存在 一 些 问 题 : 


1. 由 于 目标 应 用 平台 为 无 人 机 的 俯视 相机 ， 其 视野 内 的 物体 主要 是 
地 面 ， 而 且 相 机 的 运动 主要 为 水 平和 上 下 的 移动 ，SVO 的 许多 细节 是 围 
级 这 个 应 用 设计 的 ， 这 使 得 它 在 平视 相机 中 表现 不 佳 。 例 如 ，SVO 在 时 
月 初始 化 时 ， 使 用 了 分 解 瑟 算 阵 而 不 是 传统 的 或 E PERDI, X 
要 假设 特征 点 位 于 平面 上 。 该 假设 对 俯视 相机 是 成 立 的 ， 但 对 平视 相机 
通 负 是 不 成 立 的 ， 可 能 导致 初始 化 失败 。 再 如 ，SVO 在 关键 帧 选择 时 ， 
使 用 了 平移 量 作 为 确定 新 的 关键 帆 的 菏 略 ， 而 没有 考虑 旋转 量 。 这 同样 
在 无 人 机 俯视 配置 下 是 有 效 的 ， 但 在 平视 相机 中 则 会 容易 丢失 。 所 以 ， 
如 果 旋 者 想 要 在 平视 相机 中 使 用 $SVO， 儿 须 目 己 加 以 修改 。 


2.SVO 为 了 速度 和 轻重 化 ， 铭 工 了 后 背 优 化 和 回环 检 训 部 分 ， 也 其 
本 没有 建 图 功能 。 这 意味 看 SVO 的 位 姿 佑 计 必 然 存 在 素 积 误 震 ， 而 且 丢 
失 后 不 太 容易 进行 音 定 位 《因为 没有 搬 述 子 用 来 回环 检测 ) o MA, R 
们 称 它 为 一 个 VO， 而 不 是 称 它 为 完整 的 SLAM。 


14.1.0 RIAB-MAP 


介绍 了 几 和 款 单 目 SLAM 方 案 后 ， 我 们 再 来 看 一 些 RGB-D 传 感 器 上 的 
SLAM 方 案 。 相 比 于 单 目 和 双 目 ，RGB-D SLAM 的 原理 要 简单 很 多 OR 
党 实现 上 不 一 定 ) ， 而 且 能 够 在 CPU 上 实时 建立 稠密 的 地 图 。 


RTAB-MAP (Real Time Appearance-Based Mapping) H! 是 RGB-D 
SLAM 中 比较 经 典 的 一 个 方案 。 它 实现 了 RGB-D SLAM 中 所 有 应 该 有 的 
东西 : 基于 特征 的 视 沉 里 程 计 、 基 于 词 袋 的 回环 和 检测、 后 端的 位 姿 图 优 
化 ， 以 及 点 云 和 三 角 网 格 地 图 。 因 此 ，RTABMAP 给 出 了 一 套 完 整 的 
《但 有 些 庞大 的 ) RGB-D SLAM 方 和 案 。 目 前 我 们 已 经 可 以 直接 从 ROS 中 
获得 其 二 进 制 程序 ， 此 外 ， 在 Google Project Tango 上 也 可 以 获取 其 App 
使 用 (如 图 14-6 所 示 ) 。 


© Real-Time Appearance-Based Mapping 


First 


Third 


Top 





114-6 RTAB-MAP7EGoogle Project Tango 上 的 运行 样 例 。 


RTAB-MAP 支 持 一 些 常见 的 RGB-D 和 双 目 传感器 ， 像 Kinect、 
Xtion 等 ， 且 提供 实时 的 定位 和 建 图 功能 。 不 过 由 于 集成 度 较 高 ， 使 得 
其 他 开发 者 在 它 的 基础 上 进行 二 次 开发 变 得 困难 ， 所 以 RTAB-MAP 更 适 
合作 为 SLAM 应 用 而 非 研 究 使 用 。 


除了 这 些 开 源 方 宁 之 外 ， 庄 者 还 能 在 openslam.org 之 类 的 网 站 上 找 
到 许多 其 他 的 研究 ， 例 如 ，DVO-SLAMH271 、RGBD-SLAM-V283 
DSO ， 以 及 一 些 Kinect Fusion 相 天 的 工作 ， 等 罕 。 随 看 时 代 友 展 ， 更 
新 络 、 更 优秀 的 开源 SLAM 作 品 亦 将 出 现在 人 们 的 视野 中 ， 限 于 高 幅 这 
里 就 不 逐一 介绍 了 。 


14.2 ”未 来 的 SLAM 话题 


看 过 了 现 有 的 方案 ， 我 们 再 来 讨论 一 些 未 来 的 发 展 方 向 四 。 大 体 上 
讲 ，SLAM 将 来 的 发 展 趋 势 有 两 大 类 : 一 是 瑚 轻 量 级 、 小 型 化 方 同 发 
展 ， 让 SLAM 能 够 在 舱 入 式 或 手机 等 小 型 设备 上 民 好 运行 ， 然 后 考虑 以 
它 为 底层 功能 的 应 用 。 毕 葛 ， 大 部 分 场合 中 ， 我 们 的 真正 目的 都 是 实现 
机 器 人 、AR/VR 设 备 的 功能 ， 比 如 说 运动 、 导 航 、 教 学 、 娱 乐 ， 而 
SLAM 是 为 上 层 应 用 提供 上 自身 的 一 个 位 姿 估计 。 在 这 些 应 用 中 ， 我 们 不 
希望 SLAM 占 用 所 有 计算 资源 ， 所 以 对 SLAM 的 小 型 化 和 轻 量化 有 非常 
强烈 的 需求 。 另 一 方面 则 是 利用 高 性 能 计算 设备 ， 实 现 精 密 的 三 维 重 
建 、 场 景 理解 等 功能 。 在 这 些 应 用 中 ， 我 们 的 目的 是 完美 地 重建 场景 ， 
而 对 于 计算 资源 和 设备 的 便携 性 则 没有 多 大 限制 。 由 于 可 以 利用 GPU， 
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14.2.1 视觉 + 惯 性 导航 SLAMI 


首先 ， 我 们 要 谈 一 个 有 很 强 应 用 育 景 的 方 回 : 视觉- 惯性 导航 融合 
SLAM 方 案 。 实 际 的 机 可 人 也 好 ， 人 硬件 设备 也 好 ， 通 党 都 不 会 只 携 币 一 
种 传感器 ， 往 往 是 多 种 传 感 需 的 融合 。 学 术 界 的 研究 人 员 豆 爱 “ 大 而 且 
干净 的 问题 ”(Big Clean Problem) ， 比 如 说 仅 用 单个 摄像 头 实现 视觉 
SLAM。 但 产业 界 的 朋友 们 则 更 注重 让 算法 更 加 实用 ， 不 得 不 面 对 一 些 
复杂 而 玉 酚 的 场景 。 在 这 种 应 用 育 景 下 ， 用 视觉 与 惯性 导航 融合 进行 
SLAM 成 为 了 一 个 关注 热点 。 

IEEE GMU) Be ie iil se fe esas ASA FA RE RE, BA 
AS HANL RRRA BH SEES Be, mu HOEAPSISZJEBUZ Ja fe E 
完善 的 SLAM 系 统 028] 。 为 什么 这 么 说 呢 ? 


1.IMU 虽 然 可 以 测 得 角速度 和 加 速度 ， 但 这 些 量 都 存在 明显 的 谋 移 
CDrift) ， 使 得 积分 两 次 得 到 的 位 姿 数 据 非常 不 可 乱 。 好 比 说 ， 我 们 将 
IMU 放 在 果 上 个 动 ， 用 它 的 读数 积分 得 到 的 位 姿 也 会 党 出 十 万 八 干 里 。 
但 是 ， 对 于 短 时 间 内 的 快速 运动 ，IMU 能 够 提供 一 些 较 好 的 估计 。 这 正 
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当 运 动 过 快 时 ， CERRADO HILE RIEA, sh Pg 
同音 时 区域 太 少 以 至 于 无 法 进行 特征 匹配 ， 所 以 纯 视觉 SLAM 非 第 害怕 
快速 的 运动 。 而 有 了 IMU， 即 使 在 相机 数据 无 效 的 那 段 时 间 内 ， 我 们 也 
能 你 持 一 个 较 好 的 位 姿 信 计 ， 这 十 纯 视 党 SLAM 无 法 做 到 的 。 

2. 相 比 于 IMU， 相 机 数据 基本 不 会 有 深 移 。 如 末 相 册 放 在 原 地 固定 
不 动 ， 那 么 在 静态 场景 下 ) 视觉 SLAM 的 位 姿 佑 计 也 是 固定 不 动 的 。 
所 以 ， 相 机 数据 可 以 有 效 地 售 计 并 修正 IMU 读 数 中 的 党 移 ， 使 得 在 慢 速 
运动 后 的 位 姿 估 计 依 然 有 效 。 

3. 当 图 像 肥 生变 化 时 ， 本 质 上 我 们 没 法 知道 是 相机 目 身 及 生 了 运 
动 ， 还 是 外 界 条 件 肥 生 了 变化 ， 所 以 纯 视 涡 SLAM 难 以 处 理 动 态 的 障碍 
物 。 而 IMU 能 够 感受 到 目 己 的 运动 信息 ， 从 茶 种 程度 上 减轻 动态 物体 的 
mM 

忆 而 主 之 ， 我 们 看 到 IMU 为 快速 运动 提供 了 较 好 的 解决 方式 ， 而 相 
机 义 能 在 慢 速 运动 下 解决 IMU 的 深 移 问题 一 一 在 这 个 意义 下 ， 它 们 二 者 
re AAD HY o 
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14-7 越 来 越 多 的 相机 开始 集成 IMU 设 备 。 


当然 ， 虽 然 说 得 很 好 听 ， 不 管 是 理论 还 是 实践 ， 
VIO CVisualInertialOdometry) 都 是 相当 复杂 的 。 其 复杂 性 主要 来 源 于 
IMU 测 量 加 速度 和 角速度 这 两 个 量 的 事实 ， 所 以 不 得 不 引入 运动 学 计 
军 。 目 前 VIO 的 框 染 已 经 定型 为 两 大 类 : MA (Loosely Coupled) 和 
Zia (Tightly Coupled) 529 。 松 帮 合 是 指 IMU 和 相机 分 别 进行 目 身 的 


运动 估计 ， 然 后 对 其 位 姿 估计 结果 进行 融合 。 紧 耘 合 是 指 把 IMU 的 状态 
与 相机 的 状态 合并 在 一 起 ， 共 同 构建 运动 方程 和 观测 方程 ， 然 后 进行 状 
态 估计 一 一 这 和 我 们 之 前 介绍 的 理论 非常 相似 。 我 们 可 以 预见 ， 紧 耦合 
理论 也 必 将 分 为 基于 滤波 和 基于 优化 两 个 方 辐 。 在 滤波 方面 ， 传 统 的 
EKFU9! 以 及 改进 的 MSCKF (Multi-State Constraint KF) 1! 都 取得 了 一 
定 的 成 果 ， 研 究 者 对 EKF 也 进行 了 深入 的 讨论 (例如 能 观 性 4 ) . 优 
化 方面 亦 有 相应 的 方案 "433 。 值 得 一 提 的 是 ， 尽 管 在 纯 视 觉 SLAM 中 优 
化 方法 已 经 占 了 主流 ， 但 在 VIO 中 ， 由 于 IMU 的 数据 频率 非常 高 ， 对 状 
人 态 进 行 优 化 需要 的 计算 量 束 更 大 ， 因 此 目前 仍 处 于 滤波 与 优化 并 存 的 阶 
Bue 。 由 于 过 于 复 森 ， 限 于 篇 幅 ， 这 里 束 只 能 大 概 地 介绍 一 下 这 个 
Jn p. 

VIO 为 将 来 SLAM 的 小 型 化 与 低 成 本 化 提供 了 一 个 非常 有 效 的 方 
回 。 而 且 结 合 稀 焉 直接 法 ， 我 们 有 望 在 低 端 便 件 上 取得 民 好 的 SLAM 或 
VO 效 果 ， 是 非常 有 前 景 的 。 





14.2.2 语义 SLAM 


SLAM 的 男 一 个 大 方 癌 束 是 和 深度 学 习 技术 结合 。 到 目前 为 止 ， 
SLAM 的 方案 都 处 于 特征 点 或 者 像 系 的 层级 。 关 于 这 些 特 征 点 或 像 系 到 
底 来 自 于 什么 东西 ， 我 们 一 无 所 知 。 这 使 得 计算 机 视觉 中 的 SLAM 与 我 
们 人 类 的 做 法 不 怎么 相似 ， 至 少 我 们 上 自己 从 来 看 不 到 特征 点 ， 也 不 会 去 
根据 特征 点 判断 自身 的 运动 方向 。 我 们 看 到 的 是 一 个 个 物体 ， 通 过 左右 
眼 判 断 它 们 的 远近 ， 然 后 基于 它们 在 图 像 当 中 的 运动 推测 相机 的 移动 。 

很 久之 前 ， 人 研究 者 就 试图 将 物体 信息 结合 到 SLAM 中 。 例 如 文献 
[135,136,137,138] 中 束 兽 把 物体 识别 与 视觉 SLAM 结 合 起 来 ， 构 建 市 物体 
标签 的 地 图 。 男 一 方面 ， 把 标签 信息 引入 到 BA 或 优化 端的 目标 函数 和 
约束 中 ， 我 们 可 以 结合 特征 点 的 位 置 与 标签 信息 进行 优化 中 。 这 些 工 
作 都 可 以 称 为 语义 SLAM。 综 合 来 说 ，SLAM 和 语义 的 结合 点 主要 有 两 
个 方面 中 : 

1. 语 义 帮 助 SLAM。 传 统 的 物体 识别 、 分 割 算法 往往 只 考虑 一 幅 
图 ， 而 在 SLAM 中 我 们 拥有 一 侣 移动 的 相机 。 如 果 我 们 把 运动 过 程 中 的 


图 请 都 市 上 物体 标签 ， 吏 能 得 到 一 个 市 有 标 谷 的 地 图 。 万 外 ， 物 体 信 息 
亦 可 为 回环 检测 、BA 优 化 市 来 更 多 的 条 件 。 


2.SLAM 攻 助 语义 。 物 体 识别 和 分 割 都 需要 大 量 的 训练 数据 。 要 让 
分 类 器 识别 各 个 角度 的 物体 ， 需 要 从 不 同 视角 采集 该 物体 的 数据 ， 然 后 
进行 人 工 标定 ， 非 常 辛苦 。 而 SLAM 中 ， 由 于 我 们 可 以 估计 相机 的 运 

动 ， 可 以 自动 地 计算 物体 在 图 像 中 的 位 置 ， 节 省 人 工 标定 的 成 本 。 如 果 
有 自动 生成 的 带 高 质量 标注 的 样本 数据 ， 能 够 很 大 程度 上 加 速 分 类 器 的 
训练 过 程 。 





图 14-8 ”语义 SLAM 的 一 些 结果 ， 左 图 和 右 图 分 别 来 自 文 献 [138,140]。 


在 深度 学 习 广 泛 应 用 之 前 ， 我 们 只 能 利用 文 持 同 量 机 、 条 件 随 机 场 
等 传统 工具 对 物体 或 场景 进行 分 制 和 识别 ， 或 者 百 接 将 观测 数据 与 数据 
库 中 的 样本 进行 比较 "70 , Sea pA SEE Te xp qUSS141M2M9 。 由 于 这 些 
工具 本 刁 在 分 类 正确 率 上 存在 限制 ， 所 以 效 来 也 往往 不 尽 如 人 意 。 随 大 
深度 学 习 的 及 展 ， 我 们 开始 使 用 网 络 ， 越 来 越 准确 地 对 图 像 进行 识 别 、 
Hor DM A} 8 L46149146147149,49]. 。 这 为 构建 准确 的 语义 地 图 打下 了 更 好 的 基 
hU) 。 我 们 正 看 到 ， 逐 渭 开 始 有 学 者 将 神经 网 络 方法 引入 到 SLAM 中 
ASA a a} ll, EEBSLAMAR A HY i 28 E ET E ARS p 192159] 
。 虽 然 这 些 方法 目前 还 没有 成 为 主流 ， 但 将 SLAM 与 深度 学 习 结 合 来 处 
理 图 像 ， 亦 是 一 个 很 有 前 景 的 研究 方 回 。 


14.2.3 ”SLAM 的 未 来 


除 此 之 外 ， 基 于 线 / 面 特征 的 SLAMM54155,156] 、 动 态 场 景 下 的 
SLAM057158159 、 多 机 器 人 的 SLAM06067160 ， 等 等 ， 都 是 研究 者 感 兴趣 
并 有 发 力 的 地 方 。 按 照 包 的 观点 ， 视 党 SLAM 经 过 了 三 个 大 时 代 : 提出 问 
题 、 寻 找 算法 、 完 善 算法 。 而 我 们 目前 正 处 于 第 三 个 时 代 ， 面 对 着 如 何 
在 已 有 的 框架 中 进一步 改善 ， 使 视觉 SLAM 系 统 能 够 在 各 种 干扰 的 条 件 
下 稳定 运行 。 这 一 步 需要 许多 研究 者 的 不 懈 努 力 。 

当然 ， 没 有 人 能 够 预测 未 来 ， 我 们 也 说 不 准 会 不 会 突然 有 一 天 ， 整 
个 框架 都 被 新 的 技术 推倒 重 写 。 不 过 即使 是 那样 ， 今 天 我 们 的 付出 仍 将 
是 有 意义 的 。 没 有 今天 的 研究 ， 也 束 不 会 有 将 来 的 发 展 。 最 后 ， 和 希望 该 
者 能 在 读 完 本 书 之 后 ， 对 现 有 的 整个 SLAM 系 统 有 了 充分 的 认识 。 我 们 
也 期 待 你 能 够 为 SLAM 研 究 做 出 贡献 ! 

习题 

1. 选 择 本 讲 提 到 的 任意 一 个 开源 SLAM 系 统 ， 在 你 的 机 器 上 编译 运 
行 它 ， 直 观 体验 其 过 程 。 


2. 你 应 该 已 经 能 够 看 懂 绝 大 多 数 SLAM 相 关 论 文 了 。 拿 起 纸 和 笔 ， 
开始 你 的 研究 吧 ! 





[1] 这 是 他 博士 期 间 工 作 的 延续 。 他 现在 也 在 致力 于 将 SLAM 人 小 型 化 、 低 功率 化 。 
[2] 目前 开源 版 OFRB-SLAM 使 用 了 文本 格式 的 字典 ， 改 成 二 进 制 格式 字典 之 后 可 以 加 速 不 少 。 
[3] 这 里 有 一 部 分 是 笔者 个 人 的 理解 ， 不 一 定 完全 正确 。 





SRA ”局 斯 分 布 的 


这 里 总 结 一 下 和 常见 的 融 斯 分 布 的 性 质 ， 它 在 本 书 的 很 多 地 方 都 会 用 
到 。 

AL tt Ai 

如 果 一 个 随机 变量 x 服从 高 期 分 布 N (uo )， 那 么 它 的 概率 密度 函数 





1 1 (z — p)“ 
pm) = Tio exp (S828. | (A.1) 


1 1 Teal 
p(x) = GN det (5) exp (-5 -p)E (r-— i) (A.2) 


A.2 fay Ut ot TH A 3 gt 
A.2.1 线 性 运算 
设 两 个 独立 的 高 斯 分 布 : 
£ ~ N(B. Xu) Y ~ N(By Dyy), 


那么 ， 它 们 的 和 仍 是 高 斯 分 布 : 


ety ~ Nz + hy, Srs + Eyy). (A.3) 
如 末 以 和 常数 a 乘 以 x ， 那 么 ax WE: 
ax ~ N(ap;,a? 31,4). (A.4) 


如 果 取 y =Ax ， 那 么 y 满足 : 


y ~ N(Guz, GZ GT)， (A.5) 


A.2.2 乘 积 
设 两 个 融 斯 分 布 的 乘积 满足 p (xy )=N (u, £), WA: 
E xu dx 


i : (A.6) 


该 公式 可 以 推广 到 任意 多 个 高 斯 分 布 之 乘积 。 
A.2.38 Gis Fh 


复 
Ez) 
Hy Liye Žiyy 
由 条 件 分 布展 开 式 p (xy )=p (xly)p (y ) 可 以 推出 ， 条 件 概 率 p (xly ) 满 
AE: 


p (xly) =N (Iis jJ TM de (y = fiy) ipg = eus Se) (A.8) 


A.3 复 合 的 例子 
下 面 举 一 个 和 卡尔 曼 渡 波 鼎 相关 的 例子 。 考 虑 随机 变量 x~N (n, , X 
XX ) 为 一 变量 y Wii KE: 
y= Ax +b+w (A.9) 
其 中 Ab 为 线性 变量 的 系数 矩阵 和 偏 移 量 ，w 为 噪声 项 ， 为 零 均 值 
的 高 斯 分 布 : w~N (0,R )。 
我 们 来 看 y 的 分 布 。 根 据 前 面 的 介绍 ， 可 以 推出 : 


p(y) = N (Auz +b, R + AX,,A! + R). (A.10) 


这 为 卡尔 曼 滤 流 仑 的 预测 部 分 捉 供 了 理论 基础 。 


附录 也 ROSATI 


ROS 是 机 器 人 研究 领域 一 个 广 为 探 讨 的 主题 。 为 了 避免 使 本 书 阅读 
门槛 大 高 ， 我 们 没有 在 正文 和 例 程 中 提 到 它 。 然 而 近年 来 ，ROS 正 逐步 
在 各 大 高 校 的 学 生 中 间 得 到 推广 ， 渐 潮 为 人 们 所 熟知 和 接受 ， 所 以 这 里 
也 介绍 一 下 ROS， 硕 望 对 读者 能 有 上 所 帮助 。 

B.1ROS 是 什么 


ROS (Robot Operating System) 是 Willow Garage 公 司 于 2007 年 发 布 
的 一 个 开源 机 左 人 操作 系统 ， 它 为 软件 开 及 人 员 开 及 机 需 人 应 用 程序 提 
供 了 许多 优秀 的 工具 和 库 。 同 时 ， 还 有 优秀 的 开发 者 不 断 地 为 它 贡 献 代 
个 。 本 质 上 ，ROS 并 不 是 一 个 真正 意义 上 的 操作 系统 ， 而 更 像 是 基于 操 
作 系 统 之 上 的 一 个 软件 包 。 它 提供 了 众多 在 实际 机 上 右 人 中 可 能 过 到 的 算 
ik: 导航、 通信、 路 径 规 划 ， 等 等 。 

ROS 的 版 本 代号 是 按照 字母 须 友 来 编排 和 的， 并 随 看 Ubuntu 系 统 发 布 
更 新 。 通 常 一 个 ROS 版 本 会 支持 两 到 三 个 Ubuntu 系 统 版 本 。ROS 从 Box 
Turtle 开 始 ， 截 止 到 本 书写 作 时 (2016 年 ) ， 已 经 更 新 到 了 Kinetic 
Kame( 见 图 B-1) 。 同 时 ，ROS 也 已 经 彻底 重 构 ， 推 出 了 实时 性 更 强 的 
2.0 版 本 。 

ROSSER SERVE AS, SCR SEY ce Ubuntu ke AT AE RAS 
(Kubuntu, Linux Mint. Ubuntu GNOMES) , ， 对 其 他 Linux 发 布 版 
本 、Windows 等 的 支持 也 有 ， 不 过 没有 那么 完善 。 因 此 ， 推 荐 读者 使 用 
Ubuntu 操作 系统 来 进行 开发 和 研究 。 

ROS 文 持 目前 被 广泛 使 用 的 面 回 对 象 的 编程 语言 C++， 以 及 脚本 话 
言 Python。 你 可 以 选择 目 己 喜欢 的 语言 进行 开发 。 





«Box Turtle 








图 B-1 ROS 各 版 本 命名 方式 


B.2ROS 的 特点 


ROSH BTA, Wire A Las ATTACHE RIT LIT APE BE i 
NRF ARE Be SPE EN AN SUPE, PEAS AE n] LAS A o 

而 软件 复 用 也 正 古 软件 工程 优 关 性 最 集中 的 体现 之 一 ，ROS 能 够 以 
统一 消 居 格式 来 使 得 大 家 只 圾 要 关注 算法 层面 的 设计 ， 而 的 层 从 件 的 根 
本 目的 是 接收 各 种 各 样 的 消 居 ， 如 图 像 、 数 据守 。 各 个 便 件 厂商 将 接收 
到 的 数据 部 统一 到 ROS 所 规定 的 统一 六 居 格 式 下 ， 即 可 让 用 户 方 便 地 使 
用 各 种 开头 的 机 右 人 相关 算法 。 


在 第 14 讲 中 提 到 的 常见 的 开源 SLAM 方 案 中 ，ORB-SLAM、ORB- 
SLAM2、 LSDSLAM, SVO, DVO, RTAB-MAP, RGBD-SLAM-V2, 
Hector SLAM、Gmapping、ROVIO 等 均 有 ROS 版 本 的 开源 代码 ， 你 可 以 
很 方便 地 在 ROS 中 运行 、 调 试 和 修改 它们 。 

在 调试 SLAM 程 序 时 ， 数 据 的 来 源 通 前 有 3 种 : Tesh. BASE, 
以 及 bag 文 件 。 奋 手头 没有 相应 的 传感器 ， 通 第 瓯 需要 利用 虚拟 的 数据 
来 跑 SLAM 程 序 。 其 中 ， 最 方便 的 方式 当 属 利用 ROS 下 的 bag 文 件 友 布 
topic， 然 后 SLAM 程 序 就 可 以 监视 topic 发 出 的 数据 ， 就 像 使 用 真实 的 传 
感 器 采集 数据 一 样 。 后 面 我 们 会 简单 介绍 一 下 如 何 利 用 bag 文 件 来 模拟 
真实 的 传感器 数据 。 

B.3 如 何 快速 上 于 ROS 


ROS 有 完善 的 维基 系统 。 因 而 ， 投 照 官 网 的 介绍 在 机 需 上 安 交 对 应 


版 本 的 ROS: http://wiki.ros.org/ROS/Installation; Ja, D] BEROS E WH) 
教学 程序 即 可 。 你 会 学 习 到 ROS 的 基本 概念 、 主 题 的 发 布 和 订阅 ， 以 及 
用 Python 和 C++ 控 制 它 们 。 如 末 你 觉得 拱 烦 ， 也 可 以 使 用 针对 ROS 定 制 


的 Ubuntu: http://www.aicrobo.com/ubuntu_for_ros.html. 
除了 基本 知识 之 外 ， 你 还 可 以 学 到 一 些 ROS 的 利用 工具 ， 例 如 : 


1.rqt。rqt 是 ROS 下 的 一 个 软件 框 染 ， 它 以 插件 的 方式 提供 了 各 种 各 
样 方便 好 用 的 GUI 用 户 图 形 界 面 )。rqt 的 功能 非常 强大 ， 可 以 实时 地 
得 看 ROS 中 流动 的 消 恩 。 


2.rosbag。rosbag 古 ROS 提 供 的 一 个 非常 好 用 的 录制 及 播放 topic 数 据 
的 工具 。 当 你 想 实际 跑 一 下 SLAM 程 序 ， 但 略 于 手头 没有 实际 的 传 感 融 
时 ， 可 以 考虑 使 用 公开 提供 的 bag 文 件 来 进行 图 像 或 者 数据 的 模拟 ， 这 
种 方 陈 与 使 用 一 个 真实 的 传 感 硕 在 感觉 上 并 无 不 同 。rosbag 的 使 用 方式 
请 参考 ROS 的 维基 页 面 。 此 外 ， 许 多 公开 数据 集 也 会 提供 bag 格 式 的 数 
据 文 件 。 


3.rVviz。rviz 是 ROS 提 供 的 可 视 化 檬 块 ， 你 可 以 通过 它 实 时 地 查看 
ROS 中 的 图 像 、 点 云 、 地 图 、 规 划 的 路 人 笃 ， 等 等 ， 从 而 更 方便 地 调试 程 
序 。 


我 们 相信 ， 机 器 人 的 人 硬件 层面 和 软件 层面 一 定 都 会 问 着 统一 架构 的 
方 同 前 行 ， 而 ROS 正 是 软件 架构 层面 标准 化 一 个 重要 的 里 程 碑 。 其 中 ， 
ROS 1.x 在 之 前 被 大 量 用 于 实验 室 的 研究 ， 或 者 公司 产品 demo 的 研发 阶 
段 ， 而 ROS2 则 解决 了 ROS 实 时 性 的 问题 ， 未 来 很 有 可 能 补 直 接 用 于 实 
际 产 品 的 研发 ， 为 推进 工业 级 机 器 人 和 服务 机 器 人 的 应 用 做 出 重要 的 页 

本 附录 概述 性 地 介绍 了 有 关 ROS 的 历史 、 人 优点， 以 及 如 何 利用 ROS 
中 的 一 些 可 视 化 工具 来 辅助 SLAM 程 序 开 发 等 。 我 们 希望 读者 系统 地 学 
习 ROS， 并 使 用 ROS 开 发 自己 的 SLAM 程 序 。 
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。 博文 视点 .IT 出 版 旗舰 品牌 
re 技术 凝聚 实力 。 专业 创新 出 版 


隆重 向 读者 推荐 《视觉 SLAM 十 四 讲 : 从 理论 到 实践 》。 一 方面 这 本 书 是 业界 少 有 的 涵盖 从 基础 理论 到 
代码 实例 ， 系 统 性 讲解 SLAM 的 书 ; 另 一 方面 ， 本 书 的 作者 和 地 平 线 颇 有 渊源 ， 高 翔 曾经 是 我 们 的 算法 
实习 生 ， 颜 沁 窒 是 自动 驾驶 算法 工程 师 ， 都 是 在 SLAM 领 域 非常 杰出 的 青年 专家 ， 走 在 技术 实践 的 最 前 沿 。 
在 移动 互联 网 大 潮 之 后 ， 自 动 驶 、 无 人 机 、 服 务 机 器 人 等 人 工 智 能 硬件 会 成 为 下 一 个 产业 爆发 点 ， 其 中 
最 关键 的 技术 之 一 就 是 动态 定位 和 环境 建 模 的 SLAM 技 术 。 本 书 是 国内 最 新 、 最 有 价值 的 有 关 SLAM 技 术 
的 书籍 ， 适 合 有 志 于 从 事 机 器 人 技术 的 研究 生 和 工程 师 ， 一 定 会 让 读者 很 有 收获 。 


/分 了 地 平 线 机 器 人 创始 人 ， 中 国人 工 智能 学 会 副 秘书 长 


我 在 新 加 坡 和 加 拿 大 给 学 生 讲 视觉 SLAM 的 时 候 常常 觉得 缺乏 一 本 适合 初学 者 的 教材 。 高 翔 博士 的 《 视 
觉 SLAM 十 四 讲 : 从 理论 到 实践 》 从 最 基础 的 四 元 数 、 李 代数 讲 起 ， 涵 盖 了 卡尔 曼 滤波 、Bundle 
Adjustment、Pose-Graph 等 高 级 优化 工具 。 书 中 更 有 最 近 十 多 年 成 功 系统 的 概述 ， 从 2003 年 的 
MonoSLAM 直 到 2016 年 的 ORB-SLAM。 通 篇 既 有 清晰 的 理论 叙述 ， 又 辅 以 大 量 示例 程序 ， 是 一 本 非 
常 好 的 视觉 SLAM 教 材 。 

BY 加 拿 大 西蒙 弗 雷 泽 大 学 终身 教授 


视觉 SLAM 随 着 近年 增强 现实 、 无 人 驾驶 等 应 用 的 兴起 而 重新 获得 重大 关注 。 视 觉 SLAM 属 于 计算 机 视觉 
和 机 器 人 研究 的 交叉 领域 ， 因 此 涉及 的 基础 知识 广 而 分 散 。 国 内 专门 的 研究 机 构 相 对 较 少 ， 因 此 学 生 入 
门 的 门槛 较 高 。 幸 运 的 是 ， 本 书 不 仅 有 深入 浅 出 的 讲解 ， 同 时 注重 理论 和 实战 的 结合 ， 大 大 降低 了 国内 
学 生 和 相关 从 业者 的 进入 门槛 。 因 此 ， 本 书 非 常 值得 初学 者 学 习 实践 。 

刘海 伟 ”网 易 感 知 与 智能 中 心 增强 现实 算法 架构 师 


作者 的 这 本 书 既 是 通俗 有 趣 的 高 科技 演义 ， 又 是 足以 指导 研发 实践 的 翔实 教程 ， 对 国内 SLAM 界 而 言 可 
谓 意 义 重 大 。 我 甚至 发 现 有 不 少 目前 图 内 的 一 流 人 才 都 是 因为 看 了 本 书 的 早期 章节 才 决定 进入 这 个 行业 
并 快速 成 长 起 来 的 。 
本 书 所 涵盖 的 知识 面 、 技 术 细节 ， 甚 至 是 某 些 宝贵 的 最 优 实践 经 验 ， 对 国内 刚刚 起 步 的 虚拟 现实 和 增强 
现实 ( VR/AR) 、 无 人 机 、 无 人 车 、 机 器 人 等 行业 ， 都 将 产生 深远 影响 ! 

时 驰 (Chris) 博士 _uSens 凑 感 科 技 联合 创始 人 /首席 运营 官 








ISBN 978-7-121-511 
9 


| | | | 
Wl 


欢迎 投稿 
sz 策划 编辑 : 郑 柳 洁 邮箱 : zhengli@phei.com.cn 
Re) 责任 编辑 : A Jb ERN. Alinamercy 定价 : 75.007 


7 








